Squashed 'third_party/allwpilib/' changes from 83f1860047..f1a82828fe
f1a82828fe [wpiutil] Add DataLog and DataLogManager Stop() (#5860)
2a04e12c6f [apriltag] AprilTagFieldLayout: Add accessors for origin and field dimensions (#5869)
33e0089afb Cleanup usages of std::function<void(void)> (#5864)
d06fa633d5 [build] Fix protobuf generation when building with make (#5867)
049732afb8 [cscore] Make camera connection logging clearer (#5866)
87f7c19f90 [wpimath] Make InterpolatingDoubleTreeMap constructor public (#5865)
6b53ef47cf [wpimath] Don't recreate TrapezoidProfile in ProfiledPIDController calculate() (#5863)
8a3a268ae6 [commands] Add finallyDo with zero-arg lambda (#5862)
1c35d42cd0 [wpilib] Pop diagnostic for deprecated function use (#5859)
ddc8db6c26 [wpimath] Add feedforward constant constructor to ElevatorSim (#5823)
c6aff2c431 [upstream_utils] Update to LLVM 17.0.4 (#5855)
a9c5b18a39 [build] Update OpenCV to 2024-4.8.0-2 (#5854)
9540b6922d [hal] Add CAN IDs for AndyMark and Vivid Hosting (#5852)
83a7d33c47 [glass] Improve display of protobuf/struct type strings (#5850)
a4a8ad9c75 [commands] Make Java SelectCommand generic (#5849)
9eecf2a456 [build] Add CMake option to build Java sources jars (#5768)
9536a311cb [wpilib] Add support for the PS5 DualSense controller (#5257)
8d5e6737fc [wpilibc] SolenoidSim: Add virtual destructor (#5848)
07e13d60a2 [ntcore] Fix write_impl (#5847)
1713386869 [wpiutil] ProtobufMessageDatabase: Fix out-of-order Add() rebuild (#5845)
35472f5fc9 [ntcore] Fix a use-after-free in client close (#5844)
ed168b522c [ntcore] Disable buf pool when asan is enabled (#5843)
3e7ba2cc6f [wpinet] WebSocket: Fix write behavior (#5841)
80c47da237 [sim] Disable the robot program when DS disconnects (#5818)
abe1cec90c [wpilib] Update Usage Reporting ResourceType from NI Libraries (#5842)
cdf981abba [glass] Fix position of data type in NT view (#5840)
04dcd80adb [build] Publish unit tests for examples (#5838)
49920234ac [build] Fix checkstyle rules to allow Windows paths (#5839)
366b715942 [wpilib] Fix SendableChooser test (#5835)
3ba501f947 [commands] Java: Fix CommandXboxController.leftTrigger() parameter order (#5831)
ec569a58ef [wpimath] Make KalmanTypeFilter interface public (#5830)
b91317fd36 [wpiutil] DataLog.addSchema(): Don't add into a set view (#5829)
2ab4fcbc24 [wpiutil] ProtobufMessageDatabase: Clear messages first (#5827)
98c14f1692 [wpimath] Add EKF/UKF u-y-R correct overload (#5832)
60bcdeded9 [ci] Disable java in sanitizer builds (#5833)
c87f8fd538 [commands] Add DeferredCommand (#5566)
ad80eb3a0b [ci] Update actions for comment-command (#5824)
c7d6ad5a0b [ntcore] WebSocketConnection: Use weak capture (#5822)
8a8e220792 [simgui] Add 'Invalid' option for AllianceStation (#5820)
cfc6a47f76 [sim] DS plugin: Fix off-by-one error when setting alliance station (#5819)
8efa586ace [ntcore] Don't check type string on publishing an entry (#5816)
23ea188e60 [glass] Add protobuf decode error log message (#5812)
928e87b4f4 [build] Add combined test meta-task (#5813)
63ef585d4b [wpiutil] Fix compilation of MathExtras.h on Windows with /sdl (#5809)
b03a7668f9 [build] Windows CMake/vcpkg fixes (#5807)
3f08bcde54 [hal] Fix HAL AllianceStation on rio (#5811)
196d963dc4 [ntcore] Fix off-by-one error in stream write (#5810)
f4cbcbc984 Fix typos (NFC) (#5804)
ec0f7fefb0 [myrobot] Update the myRobot JRE (#5805)
3d618bdbfd [wpiutil] Fix Java struct array unpacking (#5801)
1fa7445667 [ntcore] Check for valid client in incoming text and binary (#5799)
269b9647da [ci] Update JDK for combine step (#5794)
bee32f080e [docs] Add wpiunits to JavaDocs (#5793)
25dad5a531 [wpinet] TCPConnector_parallel: Don't use thread_local (#5791)
4a93581f1a [build] cmake: use default library type for libglassnt, libglass, wpigui, and imgui (#5797)
abb2857e03 [wpilib] Counter: Fix default distance per pulse, add distance and rate to C++ (#5796)
b14a61e1c0 [readme] Add link to QuickBuffers release page (#5795)
cf54d9ccb7 [wpiutil, ntcore] Add structured data support (#5391)
ecb7cfa9ef [wpimath] Add Exponential motion profile (#5720)
7c6fe56cf2 [ntcore] Fix crash on disconnect (#5788)
85147bf69e [wpinet] WebSocketSerializer: Fix UB (#5787)
244163acad [wpinet] uv::Stream::TryWrite(): Return 0 on EAGAIN (#5784)
820728503d [hal] Remove extra semicolon in RoboRioData (#5786)
45f307d87e [upstream_utils] Upgrade to LLVM 17.0.3 (#5785)
4ce4d63efc [wpilibj] Fix RobotBase.isSimulation() (#5783)
579007ceb3 [commands] Add requirements parameter to Commands.idle() (#5774)
3f3a169149 [wpilib] Make physics sim setState() functions public (#5779)
7501e4ac88 [wpilib] Close sim device in ADIS IMUs (#5776)
99630d2e78 [wpimath] Upgrade to EJML 0.43.1 (#5778)
02cbbc997d [wpimath] Make Vector-Vector binary operators return Vector (#5772)
ed93889e17 [examples] Fix typo in TimesliceRobot example name (#5773)
da70e4c262 [docs] Add jinja2 to CMake prerequisites (#5771)
e814595ea7 [wpimath] Add ChassisSpeeds.fromRobotRelativeSpeeds() (#5744)
f98c943445 [wpimath] LinearSystemId: Add DCMotorSystem overload (#5770)
b3eb64b0f7 [wpiutil] ct_string: Use inline namespace for literals (#5767)
7d9ba256c2 Revert "[build] Add CMake option to build Java source jars (#5756)" (#5766)
1f6492e3d8 [sysid] Update JSON library usage (#5765)
638f04f626 [wpiutil] Add protobuf to thirdparty sources (#5746)
210255bfff [wpiutil] Update json to 3.11.2 (#5680)
896772c750 [wpimath] Add DCMotor functions for Kraken X60 and Neo Vortex (#5759)
fd427f6c82 [wpimath] Fix hardcoded module count in SwerveDriveKinematics.resetHeading() (#5762)
c0b4c6cce6 [wpimath] Add overloads for Transform2d and Transform3d (#5757)
9a0aafd8ab [examples] Make swerve examples multiply desired module speeds by cosine of heading error (#5758)
1c724884ca [build] Add CMake option to build Java source jars (#5756)
5b0db6b93e [ci] Forward CI as well (#5755)
f8cbbbac12 [ci] Take 2 on passing GITHUB_REF (#5754)
b9944be09c [ci] Pass GITHUB_REF to docker container (#5753)
de5e4eda6c [build] Update apriltag, libssh, googletest for 2024 (#5752)
227e660e20 [upstream_utils] Upgrade to LLVM 17.0.2 (#5750)
36f94c9f21 [commands,romi,xrp] Add frcYear to vendordep (#5747)
741d166457 [glass] NT view: enhance array support (#5732)
1d23513945 [ntcore] Fix string array value comparison (#5745)
ff1849052e [commands] Make command scheduling order consistent (#5470)
58e8474368 [build] Disable armsimulation unit test (#5739)
fb07b0da49 [examples] Add XRP C++ Examples and Templates (#5743)
81893ad73d Run wpiformat with clang-format 17 (#5740)
faa1e665ba [wpimath] Add ElevatorFeedforward.calculate(currentV, nextV) overload (#5715)
a789632052 [build] Update to native utils 2024.3.1 (#5738)
8f60ab5182 [build] Update OpenCV to 2024-4.8.0-1 (#5737)
33243f982b [wpimath] Expand Quaternion class with additional operators (#5600)
420f2f7c80 [ntcore] Add RTT-only subprotocol (#5731)
2b63e35ded [ntcore] Fix moving outgoing queue to new period (#5735)
be939cb636 [ntcore] Fix notification of SetDefaultEntryValue (#5733)
69a54de202 [build] Update enterprise plugin (#5730)
fef03a3ff5 [commands] Clean up C++ includes after Requirements was added (#5719)
8b7c6852cf [ntcore] Networking improvements (#5659)
1d19e09ca9 [wpiutil] Set WPI_{UN}IGNORE_DEPRECATED to empty when all else fails (#5728)
58141d6eb5 [wpilib] Make BooleanEvent more consistent (#5436)
6576d9b474 [wpilib] SendableChooser: implement Sendable instead of NTSendable (#5718)
a4030c670f [build] Update to gradle 8.4, enable win arm builds (#5727)
0960f11eba [wpinet] Revert removal of uv_clock_gettime() (#5723)
cb1bd0a3be [wpiutil] Get more precise system time on Windows (#5722)
4831277ffe [wpigui] Fix loading a maximized window on second monitor (#5721)
3eb372c25a [wpiutil] SendableBuilder: Add PublishConst methods (#5158)
1fec8596a4 [ci] Fix -dirty version (#5716)
f7e47d03f3 [build] Remove unnecessary CMake config installs (#5714)
a331ed2374 [sysid] Add SysId (#5672)
8d2cbfce16 [wpiutil] DataLog: Stop logging if insufficient free space (#5699)
48facb9cef [ntcoreffi] Add DataLogManager (#5702)
aecbcb08fc [ntcore] Correctly start DataLog for existing publishers (#5703)
5e295dfbda [wpiutil] DataLog: Limit total buffer allocation (#5700)
c7c7e05d9d [ci] Unbreak combiner (#5698)
c92bad52cb [wpilib] DataLogManager: Use system time valid function (#5697)
d404af5f24 [wpilib] RobotController: Add isSystemTimeValid() (#5696)
e56f1a3632 [ci] Run combine but skip all steps (#5695)
8f5bcad244 [ci] Use sccache for cmake builds (#5692)
703dedc4a6 [ci] Upgrade get-cmake action to fix node12 deprecation warning (#5694)
c69a0d7504 [ci] Don't run example unit test that segfaults (#5693)
66358d103e Add menu items for online docs to GUI tools (#5689)
4be8384a76 [ci] Disable combine on PR builds (#5691)
90288f06a6 [ci] Fix Gradle disk space issues (#5688)
9e9583412e [wpigui] Make wpi::gui::OpenURL() fork the process first (#5687)
d4fcd80b7b [ci] Gradle: Use container only for build step (#5684)
7b70e66772 [outlineviewer] Fix thirdparty library include sorting (#5683)
5f651df5d5 [build] Clean up Gradle configs (#5685)
65b26738d5 Add CMakeSettings.json to gitignore (#5682)
d0305951ad Fix GitHub inline warnings (#5681)
e8d4a20331 [build][cmake] Fix windows tests and re-enable CI tests (#5674)
2b58bbde0b [xrp] Add Reflectance sensor and rangefinder classes (#5673)
dd5612fbee [json] Add forward definition header (#5676)
eab44534c3 [wpimath] Remove unused SmallString include (#5677)
5ab54ff760 Replace wpi::raw_istream with wpi::MemoryBuffer (#5675)
1b6ec5a95d [wpiutil] Upgrade to LLVM 17.0.1 (#5482)
07a0d22fe6 [build] Build examples in CMake CI (#5667)
97021f074a [build] Upgrade imgui and implot (#5668)
87ce1e3761 [build] Fix wpilibNewCommands CMake install (#5671)
6ef94de9b5 [wpimath] Add tests for ArmFeedforward and ElevatorFeedforward (#5663)
c395b29fb4 [wpinet] Add WebSocket::TrySendFrames() (#5607)
c4643ba047 [romi/xrp] Fix version typo in vendordep json (#5664)
51dcb8b55a [examples] Make Romi/XRP Examples use appropriate vendordeps (#5665)
daf7702007 [build] Test each example in a new environment (#5662)
e67df8c180 [wpilib] Const-qualify EncoderSim getters (#5660)
7be290147c [wpiutil] Refactor SpanMatcher and TestPrinters from ntcore (#5658)
9fe258427a [commands] Add proxy factory to Commands (#5603)
633c5a8a22 [commands] Add C++ Requirements struct (#5504)
b265a68eea [commands] Add interruptor parameter to onCommandInterrupt callbacks (#5461)
e93c233d60 [ntcore] Compute Value memory size when creating value (#5657)
5383589f99 [wpinet] uv::Request: Return shared_ptr from Release() (#5656)
40b552be4a [wpinet] uv::Stream: Return error from TryWrite() (#5655)
202a75fe08 [wpinet] RequestImpl: Avoid infinite loop in shared_from_this() (#5654)
8896515eb7 [wpinet] uv::Buffer: Add bytes() accessor (#5653)
ae59a2fba2 [wpinet] uv::Error: Change default error to 0 (#5652)
3b51ecc35b [wpiutil] SpanExtras: Add take_back and take_front (#5651)
17f1062885 Replace std::snprintf() with wpi::format_to_n_c_str() (#5645)
bb39900353 [romi/xrp] Add Romi and XRP Vendordeps (#5644)
cb99517838 [build] cmake: Use default install location on windows for dlls (#5580)
25b0622d4c [build] Add Windows CMake CI (#5516)
34e7849605 Add warning to development builds instructions (NFC) (#5646)
e9e611c9d8 [cameraserver] Remove CameraServer.SetSize() (#5650)
94f58cc536 [wpilib] Remove Compressor.Enabled() (#5649)
4da5aee88a [wpimath] Remove SlewRateLimiter 2 argument constructor (#5648)
2e3ddf5502 Update versions in development builds instructions to 2024 (#5647)
19a8850fb1 [examples] Add TimesliceRobot templates (#3683)
9047682202 [sim] Add XRP-specific plugin (#5631)
575348b81c [wpilib] Use IsSimulation() consistently (#3534)
12e2043b77 [wpilib] Clean up Notifier (#5630)
4bac4dd0f4 [wpimath] Move PIDController from frc2 to frc namespace (#5640)
494cfd78c1 [wpiutil] Fix deprecation warning in LLVM for C++23 (#5642)
43a727e868 [apriltag] Make loadAprilTagFieldLayout throw an unchecked exception instead (#5629)
ad4b017321 [ci] Use Ninja for faster builds (#5626)
4f2114d6f5 Fix warnings from GCC 13 release build (#5637)
e7e927fe26 [build] Also compress debug info for CMake RelWithDebInfo build type (#5638)
205a40c895 [build] Specify zlib for debug info compression (#5636)
707444f000 [apriltag] Suppress -Wtype-limits warning in asserts from GCC 13 (#5635)
3b79cb6ed3 [commands] Revert SubsystemBase deprecation/removal (#5634)
bc7f23a632 [build] Compress Linux debug info (#5633)
57b2d6f254 [build] Update to image 2024 v1.0 (#5625)
339ef1ea39 [wpilib] DataLogManager: Warn user if logging to RoboRIO 1 internal storage (#5617)
7a9a901a73 [build] Fix cmake config files (#5624)
298f8a6e33 [wpilib] Add Mechanism2d tests and make Java impl match C++ (#5527)
d7ef817bae [apriltag] Update apriltag library (#5619)
c3fb31fd0e [docs] Switch to Java 17 api docs (#5613)
bd64f81cf9 [build] Run Google tests in release mode in CI (#5615)
66e6bd81ea [wpimath] Cleanup wpimath/algorithms.md (NFC) (#5621)
4fa56fd884 [build] Add missing find_dependency call (#5623)
f63d958995 [build] Update to native utils 2024.2.0 (#5601)
a9ab08f48b [wpimath] Rename ChassisSpeeds.fromDiscreteSpeeds() to discretize() (#5616)
8e05983a4a [wpimath] Add math docs to plant inversion feedforward internals (NFC) (#5618)
3a33ce918b [ntcore] Add missing StringMap include (#5620)
a6157f184d [wpiutil] timestamp: Add ShutdownNowRio (#5610)
e9f612f581 [build] Guard policy setting for CMake versions below 3.24 (#5612)
1a6df6fec6 [wpimath] Fix DARE Q decomposition (#5611)
9b3f7fb548 [build] Exclude IntelliJ folders from spotless XML (#5602)
814f18c7f5 [wpimath] Fix computation of C for DARE (A, C) detectability check (#5609)
ac23f92451 [hal] Add GetTeamNumber (#5596)
a750bee54d [wpimath] Use std::norm() in IsStabilizable() (#5599)
8e2465f8a0 [wpimath] Add arithmetic functions to wheel speeds classes (#5465)
10d4f5b5df [wpimath] Clean up notation in DARE precondition docs (#5595)
b2dd59450b [hal] Fix unfinished/incorrect GetCPUTemp functions (#5598)
99f66b1e24 [wpimath] Replace frc/EigenCore.h typedefs with Eigen's where possible (#5597)
383289bc4b [build] Make custom CMake macros use lowercase (#5594)
45e7720ec1 [build] Add error message when downloading files in CMake (#5593)
4e0d785356 [wpimath] ChassisSpeeds: document that values aren't relative to the robot (NFC) (#5551)
3c04580a57 [commands] ProxyCommand: Use inner command name in unique_ptr constructor (#5570)
cf19102c4a [commands] SelectCommand: Fix leakage and multiple composition bug (#5571)
171375f440 [ntcoreffi] Link to NI libraries (#5589)
89add5d05b Disable flaky tests (#5591)
a8d4b162ab [ntcore] Remove RPC manual tests (#5590)
39a73b5b58 [commands] C++: Add CommandPtr supplier constructor to ProxyCommand (#5572)
36d514eae7 [commands] Refactor C++ ScheduleCommand to use SmallSet (#5568)
52297ffe29 [commands] Add idle command (#5555)
67043a8eeb [wpimath] Add angular jerk unit (#5582)
51b0fb1492 [wpimath] Fix incorrect header inclusion in angular_acceleration.h (#5587)
b7657a8e28 [wpimath] Split WPIMathJNI into logical chunks (#5552)
ea17f90f87 [build] Fix tool builds with multiple arm platforms installed (#5586)
f1d7b05723 [wpimath] Clean up unit formatter (#5584)
d7264ff597 Replace wpi::errs() usage with fmtlib (#5560)
ab3bf39e0e [wpiutil] Upgrade to fmt 10.1.1 (#5585)
165ebe4c79 Upgrade to fmt 10.1.0 (#5326)
8e2a7fd306 Include thirdparty libraries with angle brackets (#5578)
e322ab8e46 [wpimath] Fix docs for DARE ABQRN stabilizability check (NFC) (#5579)
360fb835f4 [upstream_utils] Handle edge case in filename matches (#5576)
9d86624c00 [build] Fix CMake configure warnings (#5577)
969979d6c7 [wpiutil] Update to foonathan memory 0.7-3 (#5573)
0d2d989e84 [wpimath] Update to gcem 1.17.0 (#5575)
cf86af7166 [wpiutil] Update to mpack 1.1.1 (#5574)
a0c029a35b [commands] Fix dangling SelectCommand documentation (NFC) (#5567)
349141b91b [upstream_utils] Document adding a patch (NFC) (#5432)
7889b35b67 [wpimath] Add RamseteController comparison to LTV controller docs (NFC) (#5559)
b3ef536677 [build] Ignore nt/sim json files in spotless (#5565)
ed895815b5 [build] Compile Java with UTF-8 encoding (#5564)
2e4ad35e36 [wpiutil] jni_util: Add JSpan and CriticalJSpan (#5554)
8f3d6a1d4b [wpimath] Remove discretizeAQTaylor() (#5562)
7c20fa1b18 [wpimath] Refactor DARE tests to reduce RAM usage at compile time (#5557)
89e738262c [ntcore] Limit buffer pool size to 64KB per connection (#5485)
96f7fa662e Upgrade Maven dependencies (#5553)
7a2d336d52 [wpinet] Leak multicast handles during windows shutdown (#5550)
f9e2757d8f [wpimath] Use JDoubleArrayRef in all JNI functions (#5546)
0cf6e37dc1 [wpimath] Make LTV controller constructors use faster DARE solver (#5543)
6953a303b3 [build] Fix the windows build with fmt (#5544)
7a37e3a496 [wpimath] Correct Rotation3d::RotateBy doc comment (NFC) (#5541)
186b409e16 [wpimath] Remove internal Eigen header include (#5539)
03764dfe93 [wpimath] Add static matrix support to DARE solver (#5536)
394cfeadbd [wpimath] Use SDA algorithm instead of SSCA for DARE solver (#5526)
a4b7fde767 [wpilib] Add mechanism specific SetState overloads to physics sims (#5534)
8121566258 [wpimath] Fix CoordinateSystem.convert() Transform3d overload (#5532)
b542e01a0b [glass] Fix array crash when clearing existing workspace (#5535)
e2e1b763b2 [wpigui] Fix PFD file dialogs not closing after window closing (#5530)
86d7bbc4e4 [examples] Add Java Examples and Templates for the XRP (#5529)
e8b5d44752 [wpimath] Make Java Quaternion use doubles instead of Vector (#5525)
38c198fa64 [myRobot] Add apriltags to myRobot build (#5528)
00450c3548 [wpimath] Upgrade to EJML 0.42 (#5531)
faf3cecd83 [wpimath] Don't copy Matrix and underlying storage in VecBuilder (#5524)
6b896a38dc [build] Don't enforce WITH_FLAT_INSTALL with MSVC (part 2) (#5517)
c01814b80e [wpiutil] Add C API for DataLog (#5509)
b5bd0771eb [wpimath] Document extrinsic vs intrinsic rotations (NFC) (#5508)
84ed8aec05 [build] Don't enforce WITH_FLAT_INSTALL with MSVC (#5515)
999f677d8c [ntcoreffi] Add WPI_Impl_SetupNowRio to exported symbols (#5510)
338f37d302 Fix header sorting of libssh (#5507)
75cbd9d6d0 [glass] Add background color selector to glass plots (#5506)
e2c190487b [examples] Add flywheel bang-bang controller example (#4071)
c52dad609e [wpinet] WebSocket: Send pong in response to ping (#5498)
e2d17a24a6 [hal] Expose power rail disable and cpu temp functionality (#5477)
3ad5d2e42d [hal,wpiutil] Use HMB for FPGA Timestamps (#5499)
b46a872494 [ntcore] Remove pImpl from implementation (#5480)
d8c59ccc71 [wpimath] Add tests for MathUtil clamp() and interpolate() (#5501)
0552c8621d [glass,ov] Improve Glass and OutlineViewer title bar message (#5502)
90e37a129f [wpiutil,wpimath] Add generic InterpolatingTreeMap (#5372)
d83a6edc20 [wpilib] Update GetMatchTime docs and units (#5232)
6db2c42966 [wpimath] Trajectory: Throw on empty lists of States (#5497)
21439b606c [wpimath] Disallow LTV controller max velocities above 15 m/s (#5495)
7496e0d208 [ntcore] Value: More efficiently store arrays (#5484)
0c93aded8a [wpimath] Change kinematics.ToTwist2d(end - start) to kinematics.ToTwist2d(start, end) (#5493)
815a8403e5 [wpimath] Give infeasible trajectory constraints a better exception message (#5492)
35a8b129d9 [wpimath] Add RotateBy() function to pose classes (#5491)
26d6e68c8f [upstream_utils] Add GCEM to CI (#5483)
6aa469ae45 [wpilib] Document how to create LinearSystem object for physics sim classes (NFC) (#5488)
a01b6467d3 [wpimath] Link to docs on LQR and KF tolerances (#5486)
d814f1d123 [wpimath] Fix copy-paste error from Pose2d docs (NFC) (#5490)
98f074b072 [wpimath] Add folder prefix to geometry includes (#5489)
e9858c10e9 [glass] Add tooltips for NT settings (#5476)
12dda24f06 [examples] Fix C robot template not correctly looping (#5474)
fc75d31755 [apriltag] Update apriltaglib (#5475)
a95994fff6 [wpiutil] timestamp: Call FPGA functions directly (#5235)
2ba8fbb6f4 [wpimath] Improve documentation for SwerveModulePosition::operator- (#5468)
b8cdf97621 [build] Prepare for Windows arm64 builds (#5390)
552f4b76b5 [wpimath] Add FOC-enabled Falcon constants to the DCMotor class (#5469)
1938251436 [examples] Add Feedforward to ElevatorProfiledPid (#5300)
873c2a6c10 [examples] Update ElevatorTrapezoidProfile example (#5466)
99b88be4f3 [wpilib] Reduce usage of NTSendable (#5434)
d125711023 [hal] Fix Java REVPH faults bitfield (take 2) (#5464)
c3fab7f1f2 [ntcore] Don't update timestamp when value is unchanged (#5356)
5ec7f18bdc [wpilib] EventLoop docs: Remove BooleanEvent references (NFC) (#5463)
c065ae1fcf [wpiunits] Add subproject for a Java typesafe unit system (#5371)
44acca7c00 [wpiutil] Add ClassPreloader (#5365)
88b11832ec [hal] Fix Java REVPH faults bitfield (#5148)
fb57d82e52 [ntcore] Enhance Java raw value support
3a6e40a44b [wpiutil] Enhance DataLog Java raw value support
8dae5af271 [wpiutil] Add compile-time string utilities (ct_string) (#5462)
fc56f8049a [wpilib] DriverStation: Change alliance station to use optional (#5229)
ef155438bd [build] Consume libuv via cmake config instead of via pkg-config (#5438)
86e91e6724 [wpimath] Refactor TrapezoidProfile API (#5457)
72a4543493 [wpilib] DutyCycleEncoderSim: Expand API (#5443)
657338715d [wpimath] Add ChassisSpeeds method to fix drifting during compound swerve drive maneuvers (#5425)
1af224c21b Add missing <functional> includes (#5459)
0b91ca6d5a [wpilib] SendableChooser: Add onChange listener (#5458)
6f7cdd460e [wpimath] Pose3d: Switch to JNI for exp and log (#5444)
c69e34c80c [wpimath] ChassisSpeeds: Add arithmetic functions (#5293)
335e7dd89d [wpilib] Simulation: Add ctor parameter to set starting state of mechanism sims (#5288)
14f30752ab [wpilib] Deprecate Accelerometer and Gyro interfaces (#5445)
70b60e3a74 [commands] Trigger: Fix method names in requireNonNullParam (#5454)
593767c8c7 [wpimath] Improve Euler angle calculations in gimbal lock (#5437)
daf022d3da [build] Make devImplementation inherit from implementation (#5450)
9b8d90b852 [examples] Convert the unitless joystick inputs to actual physical units (#5451)
1f6428ab63 [ntcore] Fix undefined comparison behavior when array is empty (#5448)
17eb9161cd Update code owners for removal of old commands (#5447)
3c4b58ae1e [wpinet] Upgrade to libuv 1.46.0 (#5446)
aaea85ff16 [commands] Merge CommandBase into Command and SubsystemBase into Subsystem (#5392)
7ac932996a [ci] Use PAT for workflow dispatch (#5442)
efe1987e8b [ci] Trigger pages repo workflow (#5441)
828bc5276f [wpiutil] Upgrade to LLVM 16.0.6 (#5435)
701df9eb87 [ci] Change documentation publish to single-commit (#5440)
e5452e3f69 [wpiutil] Add WPICleaner and an example how to use it (#4850)
7a099cb02a [commands] Remove deprecated classes and functions (#5409)
b250a03944 [wpilib] Add function to wait for DS Connection (#5230)
a6463ed761 [wpiutil] Fix unused variable warning in release build (#5430)
f031513470 [ntcore] NetworkTable::GetSubTables(): Remove duplicates (#5076)
f8e74e2f7c [hal] Unify PWM simulation Speed, Position, and Raw (#5277)
fd5699b240 Remove references to Drake (#5427)
e2d385d80a [build] cmake: Respect USE_SYSTEM_FMTLIB (#5429)
d37f990ce3 [hal] Fix HAL Relay/Main doc module (NFC) (#5422)
a7a8b874ac [docs] Expand HAL_ENUM in doxygen docs (#5421)
3a61deedde [wpimath] Rotation2d: Only use gcem::hypot when constexpr evaluated (#5419)
96145de7db [examples] Fix formatting (NFC) (#5420)
fffe6a7b9a [examples] Improve Pneumatics example coverage in Solenoid and RapidReactCmdBot examples (#4998)
6b5817836d [wpimath] Add tolerance for some tests (#5416)
3233883f3e [cscore] Fix warnings on macos arm (#5415)
c4fc21838f [commands] Add ConditionalCommand getInterruptionBehavior (#5161)
89fc51f0d4 Add tests for SendableChooser and Command Sendable functionality (#5179)
663bf25aaf [docs] Generate docs for symbols in __cplusplus (#5412)
fe32127ea8 [command] Clean up Command doc comments (NFC) (#5321)
c1a01569b4 [wpilib][hal] PWM Raw using microseconds (#5283)
1fca519fb4 [wpiutil] Remove remnants of ghc fs and tcb_span libraries (#5411)
90602cc135 [github] Update issue template to collect more project info (#5090)
34412ac57e [build] Exclude files in bin from Spotless (#5410)
61aa60f0e3 [wpilib] Add robot callback that is called when the DS is initially connected (#5231)
ebae341a91 [commands] Add test for subsystem registration and periodic (#5408)
5d3a133f9f Remove spaces in NOLINT comments (#5407)
3a0e484691 [wpimath] Fix clang-tidy warnings (#5403)
eb3810c765 [wpiutil] Fix clang-tidy warnings (#5406)
c4dc697192 [hal] WS Simulation: Add message filtering capability (#5395)
0eccc3f247 [ntcore] Fix clang-tidy warnings (#5405)
f4dda4bac0 [hal] Add javadocs for JNI (NFC) (#5298)
1c20c69793 [cscore] Fix clang-tidy warnings (#5404)
1501607e48 [commands] Fix clang-tidy warnings (#5402)
991f4b0f62 [wpimath] PIDController: Add IZone (#5315)
f5b0d1484b [wpimath] Add isNear method to MathUtil (#5353)
2ce248f66c [hal] Fix clang-tidy warnings (#5401)
5fc4aee2d2 [wpimath] SwerveDriveKinematics: Rename currentChassisSpeed to desiredChassisSpeed (#5393)
50b90ceb54 [wpimath] SwerveDriveKinematics: Add reset method (#5398)
316cd2a453 [commands] Notify DriverStationSim in CommandTestBaseWithParam (#5400)
d4ea5fa902 [cscore] VideoMode: Add equals override (Java) (#5397)
d6bd72d738 [wpimath] ProfiledPIDController: Add getConstraints (#5399)
25ad5017a9 [wpimath] Refactor kinematics, odometry, and pose estimator (#5355)
5c2addda0f [doc] Add missing pneumatics docs (NFC) (#5389)
c3e04a6ea2 Fix loading tests on macos 12 (#5388)
d5ed9fb859 [wpimath] Create separate archive with just units headers (#5383)
901ab693d4 [wpimath] Use UtilityClassTest for more utility classes (#5384)
9d53231b01 [wpilib] DataLogManager: Add warning for low storage space (#5364)
d466933963 [wpiutil] Group doxygen into MPack module (#5380)
652d1c44e3 [wpiutil] Upgrade to macOS 12 to remove concept shims (#5379)
6414be0e5d [wpimath] Group units doxygen modules (#5382)
7ab5800487 [wpiutil] Fix docs typo in SmallVector (#5381)
59905ea721 Replace WPI_DEPRECATED() macro with [[deprecated]] attribute (#5373)
753cb49a5e [ntcore] Fix doxygen module in generated C types (NFC) (#5374)
1c00a52b67 [hal] Expose CAN timestamp base clock (#5357)
91cbcea841 Replace SFINAE with concepts (#5361)
d57d1a4598 [wpimath] Remove unnecessary template argument from unit formatter (#5367)
5acc5e22aa [wpimath] Only compute eigenvalues with EigenSolvers (#5369)
d3c9316a97 extend shuffleboard test timeout (#5377)
1ea868081a [ci] Fix /format command (#5376)
5fac18ff4a Update formatting to clang-format 16 (#5370)
a94a998002 [wpimath] Generalize Eigen formatter (#5360)
125f6ea101 [wpimath] Make SwerveDriveKinematics::ToChassisSpeeds() take const-ref argument (#5363)
51066a5a8a [wpimath] Move unit formatters into units library (#5358)
282c032b60 [wpilibc] Add unit-aware Joystick.GetDirection() (#5319)
073d19cb69 [build] Fix CMake warning (#5359)
01490fc77b [wpiutil] DataLog: Add documentation for append methods (NFC) (#5348)
c9b612c986 [wpilibcExamples] Make C++ state-space elevator KF and LQR match Java (#5346)
eed1e6e3cb [wpimath] Replace DiscretizeAQTaylor() with DiscretizeAQ() (#5344)
c976f40364 [readme] Document how to run examples in simulation (#5340)
4d28bdc19e [ci] Update Github Pages deploy action parameters (#5343)
e0f851871f [ci] Fix github pages deploy version (#5342)
063c8cbedc Run wpiformat (NFC) (#5341)
96e41c0447 [ci] Update deploy and sshagent actions (#5338)
fd294bdd71 [build] Fix compilation with GCC 13 (#5322)
d223e4040b [dlt] Add delete without download functionality (#5329)
abc19bcb43 [upstream_utils] Zero out commit hashes and show 40 digits in index hashes (#5336)
e909f2e687 [build] Update gradle cache repo name (#5334)
52bd5b972d [wpimath] Rewrite DARE solver (#5328)
3876a2523a [wpimath] Remove unused MatrixImpl() function (#5330)
c82fcb1975 [wpiutil] Add reflection based cleanup helper (#4919)
15ba95df7e [wpiutil] Use std::filesystem (#4941)
77c2124fc5 [wpimath] Remove Eigen's custom STL types (#4945)
27fb47ab10 [glass] Field2D: Embed standard field images (#5159)
102e4f2566 [wpilib] Remove deprecated and broken SPI methods (#5249)
463a90f1df [wpilib, hal] Add function to read the RSL state (#5312)
7a90475eec [wpilib] Update RobotBase documentation (NFC) (#5320)
218cfea16b [wpilib] DutyCycleEncoder: Fix reset behavior (#5287)
91392823ff [build] Update to gradle 8.1 (#5303)
258b7cc48b [wpilibj] Filesystem.getDeployDirectory(): Strip JNI path from user.dir (#5317)
26cc43bee1 [wpilib] Add documentation to SPI mode enum (NFC) (#5324)
ac4da9b1cb [hal] Add HAL docs for Addressable LED (NFC) (#5304)
21d4244cf7 [wpimath] Fix DCMotor docs (NFC) (#5309)
1dff81bea7 [hal] Miscellaneous HAL doc fixes (NFC) (#5306)
7ce75574bf [wpimath] Upgrade to Drake v1.15.0 (#5310)
576bd646ae [hal] Add CANManufacturer for Redux Robotics (#5305)
ee3b4621e5 [commands] Add onlyWhile and onlyIf (#5291)
40ca094686 [commands] Fix RepeatCommand calling end() twice (#5261)
9cbeb841f5 [rtns] Match imaging tool capitalization (#5265)
a63d06ff77 [examples] Add constants to java gearsbot example (#5248)
b6c43322a3 [wpilibc] XboxController: Add return tag to docs (NFC) (#5246)
5162d0001c [hal] Fix and document addressable LED timings (#5272)
90fabe9651 [wpilibj] Use method references in drive class initSendable() (#5251)
24828afd11 [wpimath] Fix desaturateWheelSpeeds to account for negative speeds (#5269)
e099948a77 [wpimath] Clean up rank notation in docs (NFC) (#5274)
fd2d8cb9c1 [hal] Use std::log2() for base-2 logarithm (#5278)
ba8c64bcff [wpimath] Fix misspelled Javadoc parameters in pose estimators (NFC) (#5292)
f53c6813d5 [wpimath] Patch Eigen warnings (#5290)
663703d370 [gitattributes] Mark json files as lf text files (#5256)
aa34aacf6e [wpilib] Shuffleboard: Keep duplicates on SelectTab() (#5198)
63512bbbb8 [wpimath] Fix potential divide-by-zero in RKDP (#5242)
9227b2166e [wpilibj] DriverStation: Fix joystick data logs (#5240)
fbf92e9190 [wpinet] ParallelTcpConnector: don't connect to duplicate addresses (#5169)
2108a61362 [ntcore] NT4 client: close timed-out connections (#5175)
0a66479693 [ntcore] Optimize scan of outgoing messages (#5227)
b510c17ef6 [hal] Fix RobotController.getComments() mishandling quotes inside the comments string (#5197)
e7a7eb2e93 [commands] WaitCommand: Remove subclass doc note (NFC) (#5200)
a465f2d8f0 [examples] Shuffleboard: Correct parameter order (#5204)
a3364422fa LICENSE.md: Bump year to 2023 (#5195)
df3242a40a [wpimath] Fix NaN in C++ MakeCostMatrix() that takes an array (#5194)
00abb8c1e0 [commands] RamseteCommand: default-initialize m_prevSpeeds (#5188)
c886273fd7 [wpilibj] DutyCycleEncoder.setDistancePerRotation(): fix simulation (#5147)
53b5fd2ace [ntcore] Use int64 for datalog type string (#5186)
56b758320f [wpilib] DataLogManager: increase time for datetime to be valid (#5185)
08f298e4cd [wpimath] Fix Pose3d log returning Twist3d NaN for theta between 1E-8 and 1E-7 (#5168)
6d0c5b19db [commands] CommandScheduler.isComposed: Remove incorrect throws clause (NFC) (#5183)
0d22cf5ff7 [wpilib] Fix enableLiveWindowInTest crashing in disabled (#5173)
32ec5b3f75 [wpilib] Add isTestEnabled and minor docs cleanup (#5172)
e5c4c6b1a7 [wpimath] Fix invalid iterator access in TimeInterpolatableBuffer (#5138)
099d048d9e [wpimath] Fix Pose3d log returning Twist3d NaN for theta between 1E-9 and 1E-8 (#5143)
4af84a1c12 Fix Typos (NFC) (#5137)
ce3686b80d [wpimath] Check LTV controller max velocity precondition (#5142)
4b0eecaee0 [commands] Subsystem: Add default command removal method (#5064)
edf4ded412 [wpilib] PH: Revert to 5V rail being fixed 5V (#5122)
4c46b6aff9 [wpilibc] Fix DataLogManager crash on exit in sim (#5125)
490ca4a68a [wpilibc] Fix XboxController::GetBackButton doc (NFC) (#5131)
cbb5b0b802 [hal] Simulation: Fix REV PH solenoids 8+ (#5132)
bb7053d9ee [hal] Fix HAL_GetRuntimeType being slow on the roboRIO (#5130)
9efed9a533 Update .clang-format to c++20 (#5121)
dbbfe1aed2 [wpilib] Use PH voltage to calc Analog pressure switch threshold (#5115)
de65a135c3 [wpilib] DutyCycleEncoderSim: Add channel number constructor (#5118)
3e9788cdff [docs] Strip path from generated NT docs (#5119)
ecb072724d [ntcore] Client::Disconnect(): actually close connection (#5113)
0d462a4561 [glass] NT view: Change string/string array to quoted (#5111)
ba37986561 [ntcore] NetworkClient::Disconnect: Add null check (#5112)
25ab9cda92 [glass,ov] Provide menu item to create topic from root (#5110)
2f6251d4a6 [glass] Set default value when publishing new topic (#5109)
e9a7bed988 [wpimath] Add timestamp getter to MathShared (#5091)
9cc14bbb43 [ntcore] Add stress test to dev executable (#5107)
8068369542 [wpinet] uv: Stop creating handles when closing loop (#5102)
805c837a42 [ntcore] Fix use-after-free in server (#5101)
fd18577ba0 [commands] Improve documentation of addRequirements (NFC) (#5103)
74dea9f05e [wpimath] Fix exception for empty pose buffer in pose estimators (#5106)
9eef79d638 [wpilib] PneumaticHub: Document range of enableCompressorAnalog (NFC) (#5099)
843574a810 [ntcore] Use wpi::Now instead of loop time for transmit time
226ef35212 [wpinet] WebSocket: Reduce server send frame overhead
b30664d630 [ntcore] Reduce initial connection overhead
804e5ce236 [examples] MecanumDrive: Fix axis comment in C++ example (NFC) (#5096)
49af88f2bb [examples] ArmSimulation: Fix flaky test (#5093)
d56314f866 [wpiutil] Disable mock time on the Rio (#5092)
43975ac7cc [examples] ArmSimulation, ElevatorSimulation: Extract mechanism to class (#5052)
5483464158 [examples, templates] Improve descriptions (NFC) (#5051)
785e7dd85c [wpilibc] SendableChooser: static_assert copy- and default-constructibility (#5078)
e57ded8c39 [ntcore] Improve disconnect error reporting (#5085)
01f0394419 [wpinet] Revert WebSocket: When Close() is called, call closed immediately (#5084)
59be120982 [wpimath] Fix Pose3d exp()/log() and add rotation vector constructor to Rotation3d (#5072)
37f065032f [wpilib] Refactor TimedRobot tests (#5068)
22a170bee7 [wpilib] Add Notifier test (#5070)
2f310a748c [wpimath] Fix DCMotor.getSpeed() (#5061)
b43ec87f57 [wpilib] ElevatorSim: Fix WouldHitLimit methods (#5057)
19267bef0c [ntcore] Output warning on property set on unpublished topic (#5059)
84cbd48d84 [ntcore] Handle excludeSelf on SetDefault (#5058)
1f35750865 [cameraserver] Add GetInstance() to all functions (#5054)
8230fc631d [wpilib] Revert throw on nonexistent SimDevice name in SimDeviceSim (#5053)
b879a6f8c6 [wpinet] WebSocket: When Close() is called, call closed immediately (#5047)
49459d3e45 [ntcore] Change wire timeout to fixed 1 second (#5048)
4079eabe9b [wpimath] Discard stale pose estimates (#5045)
fe5d226a19 [glass] Fix option for debug-level NT logging (#5049)
b7535252c2 [ntcore] Don't leak buffers in rare WS shutdown case (#5046)
b61ac6db33 [ntcore] Add client disconnect function (#5022)
7b828ce84f [wpimath] Add nearest to Pose2d and Translation2d (#4882)
08a536291b [examples] Improvements to Elevator Simulation Example (#4937)
193a10d020 [wpigui] Limit frame rate to 120 fps by default (#5030)
7867bbde0e [wpilib] Clarify DS functions provided by FMS (NFC) (#5043)
fa7c01b598 [glass] Add option for debug-level NT logging (#5007)
2b81610248 [wpiutil] Add msgpack to datalog Python example (#5032)
a4a369b8da CONTRIBUTING.md: Add unicodeit CLI to math docs guidelines (#5031)
d991f6e435 [wpilib] Throw on nonexistent SimDevice name in SimDeviceSim constructor (#5041)
a27a047ae8 [hal] Check for null in getSimDeviceName JNI (#5038)
2f96cae31a [examples] Hatchbots: Add telemetry (#5011)
83ef8f9658 [simulation] GUI: Fix buffer overflow in joystick axes copy (#5036)
4054893669 [commands] Fix C++ Select() factory (#5024)
f75acd11ce [commands] Use Timer.restart() (#5023)
8bf67b1b33 [wpimath] PIDController::Calculate(double, double): update setpoint flag (#5021)
49bb1358d8 [wpiutil] MemoryBuffer: Fix GetMemoryBufferForStream (#5017)
9c4c07c0f9 [wpiutil] Remove NDEBUG check for debug-level logging (#5018)
1a47cc2e86 [ntcore] Use full handle when subscribing (#5013)
7cd30cffbc Ignore networktables.json (#5006)
92aecab2ef [commands] Command controllers are not subclasses (NFC) (#5000)
8785bba080 [ntcore] Special-case default timestamps (#5003)
9e5b7b8040 [ntcore] Handle topicsonly followed by value subscribe (#4991)
917906530a [wpilib] Add Timer::Restart() (#4963)
00aa66e4fd [wpimath] Remove extraneous assignments from DiscretizeAB() (#4967)
893320544a [examples] C++ RamseteCommand: Fix units (#4954)
b95d0e060d [wpilib] XboxController: Fix docs discrepancy (NFC) (#4993)
008232b43c [ntcore] Write empty persistent file if none found (#4996)
522be348f4 [examples] Rewrite tags (NFC) (#4961)
d48a83dee2 [wpimath] Update Wikipedia links for quaternion to Euler angle conversion (NFC) (#4995)
504fa22143 [wpimath] Workaround intellisense Eigen issue (#4992)
b2b25bf09f [commands] Fix docs inconsistency for toggleOnFalse(Command) (NFC) (#4978)
ce3dc4eb3b [hal] Properly use control word that is in sync with DS data (#4989)
1ea48caa7d [wpilib] Fix C++ ADXRS450 and Java SPI gyro defs (#4988)
fb101925a7 [build] Include wpimathjni in commands binaries (#4981)
657951f6dd [starter] Add a process starter for use by the installer for launching tools (#4931)
a60ca9d71c [examples] Update AprilTag field load API usage (#4975)
f8a45f1558 [wpimath] Remove print statements from tests (#4977)
ecba8b99a8 [examples] Fix swapped arguments in MecanumControllerCommand example (#4976)
e95e88fdf9 [examples] Add comment to drivedistanceoffboard example (#4877)
371d15dec3 [examples] Add Computer Vision Pose Estimation and Latency Compensation Example (#4901)
cb9b8938af [sim] Enable docking in the GUI (#4960)
3b084ecbe0 [apriltag] AprilTagFieldLayout: Improve API shape for loading builtin JSONs (#4949)
27ba096ea1 [wpilib] Fix MOI calculation error in SingleJointedArmSim (#4968)
42c997a3c4 [wpimath] Fix Pose3d exponential and clean up Pose3d logarithm (#4970)
5f1a025f27 [wpilibj] Fix typo in MecanumDrive docs (NFC) (#4969)
0ebf79b54c [wpimath] Fix typo in Pose3d::Exp() docs (NFC) (#4966)
a8c465f3fb [wpimath] HolonomicDriveController: Add getters for the controllers (#4948)
a7b1ab683d [wpilibc] Add unit test for fast deconstruction of GenericHID (#4953)
bd6479dc29 [build] Add Spotless for JSON (#4956)
5cb0340a8c [hal, wpilib] Load joystick values upon code initialization (#4950)
ab0e8c37a7 [readme] Update build requirements (NFC) (#4947)
b74ac1c645 [build] Add apriltag to C++ cmake example builds (#4944)
cf1a411acf [examples] Add example programs for AprilTags detection (#4932)
1e05b21ab5 [wpimath] Fix PID atSetpoint to not return true prematurely (#4906)
e5a6197633 [wpimath] Fix SwerveDriveKinematics not initializing a new array each time (#4942)
039edcc23f [ntcore] Queue current value on subscriber creation (#4938)
f7f19207e0 [wpimath] Allow multiple vision measurements from same timestamp (#4917)
befd12911c [commands] Delete UB-causing rvalue variants of CommandPtr methods (#4923)
34519de60a [commands] Fix spacing in command composition exception (#4924)
dc4355c031 [hal] Add handle constructor and name getters for sim devices (#4925)
53d8d33bca [hal, wpilibj] Add missing distance per pulse functions to EncoderSim (#4928)
530ae40614 [apriltag] Explain what April tag poses represent (NFC) (#4930)
79f565191e [examples] DigitalCommunication, I2CCommunication: Add tests (#4865)
2cd9be413f [wpilib, examples] Cleanup PotentiometerPID, Ultrasonic, UltrasonicPID examples (#4893)
babb0c1fcf [apriltag] Add 2023 field layout JSON (#4912)
330ba45f9c [wpimath] Fix swerve kinematics util classes equals function (#4907)
51272ef6b3 [fieldImages] Add 2023 field (#4915)
0d105ab771 [commands] Deduplicate command test utils (#4897)
cf4235ea36 [wpiutil] Guard MSVC pragma in SymbolExports.h (#4911)
2d4b7b9147 [build] Update opencv version in opencv.gradle (#4909)
aec6f3d506 [ntcore] Fix client flush behavior (#4903)
bfe346c76a [build] Fix cmake java resources (#4898)
Change-Id: Ia1dd90fe42c6cd5df281b8a5b710e136f54355f4
git-subtree-dir: third_party/allwpilib
git-subtree-split: f1a82828fed8950f9a3f1586c44327027627a0c8
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/sysid/.styleguide b/sysid/.styleguide
new file mode 100644
index 0000000..8506bf1
--- /dev/null
+++ b/sysid/.styleguide
@@ -0,0 +1,35 @@
+cppHeaderFileInclude {
+ \.h$
+ \.inc$
+ \.inl$
+}
+
+cppSrcFileInclude {
+ \.cpp$
+}
+
+generatedFileExclude {
+ src/main/native/resources/
+ src/main/native/win/sysid.ico
+ src/main/native/mac/sysid.icns
+}
+
+repoRootNameOverride {
+ sysid
+}
+
+includeOtherLibs {
+ ^GLFW
+ ^fmt/
+ ^frc/
+ ^glass/
+ ^gtest/
+ ^imgui
+ ^implot\.h$
+ ^networktables/
+ ^portable-file-dialogs\.h$
+ ^ntcore
+ ^units/
+ ^wpi/
+ ^wpigui
+}
diff --git a/sysid/CMakeLists.txt b/sysid/CMakeLists.txt
new file mode 100644
index 0000000..31fbe08
--- /dev/null
+++ b/sysid/CMakeLists.txt
@@ -0,0 +1,41 @@
+project(sysid)
+
+include(CompileWarnings)
+include(GenResources)
+include(LinkMacOSGUI)
+include(AddTest)
+
+configure_file(src/main/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
+generate_resources(src/main/native/resources generated/main/cpp SYSID sysid sysid_resources_src)
+
+file(GLOB_RECURSE sysid_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
+
+if (WIN32)
+ set(sysid_rc src/main/native/win/sysid.rc)
+elseif(APPLE)
+ set(MACOSX_BUNDLE_ICON_FILE sysid.icns)
+ set(APP_ICON_MACOSX src/main/native/mac/sysid.icns)
+ set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
+endif()
+
+add_executable(sysid ${sysid_src} ${sysid_resources_src} ${sysid_rc} ${APP_ICON_MACOSX})
+wpilib_link_macos_gui(sysid)
+wpilib_target_warnings(sysid)
+target_include_directories(sysid PRIVATE src/main/native/include)
+target_link_libraries(sysid wpimath libglassnt libglass)
+
+if (WIN32)
+ set_target_properties(sysid PROPERTIES WIN32_EXECUTABLE YES)
+elseif(APPLE)
+ set_target_properties(sysid PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "SysId")
+endif()
+
+if (WITH_TESTS)
+ wpilib_add_test(sysid src/test/native/cpp)
+ wpilib_link_macos_gui(sysid_test)
+ target_sources(sysid_test PRIVATE ${sysid_src})
+ target_compile_definitions(sysid_test PRIVATE RUNNING_SYSID_TESTS)
+ target_include_directories(sysid_test PRIVATE src/main/native/cpp
+ src/main/native/include)
+ target_link_libraries(sysid_test wpimath libglassnt libglass gtest)
+endif()
diff --git a/sysid/Info.plist b/sysid/Info.plist
new file mode 100644
index 0000000..37bb798
--- /dev/null
+++ b/sysid/Info.plist
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+ <key>CFBundleName</key>
+ <string>SysId</string>
+ <key>CFBundleExecutable</key>
+ <string>sysid</string>
+ <key>CFBundleDisplayName</key>
+ <string>SysId</string>
+ <key>CFBundleIdentifier</key>
+ <string>edu.wpi.first.tools.SysId</string>
+ <key>CFBundleIconFile</key>
+ <string>sysid.icns</string>
+ <key>CFBundlePackageType</key>
+ <string>APPL</string>
+ <key>CFBundleSupportedPlatforms</key>
+ <array>
+ <string>MacOSX</string>
+ </array>
+ <key>CFBundleInfoDictionaryVersion</key>
+ <string>6.0</string>
+ <key>CFBundleShortVersionString</key>
+ <string>2021</string>
+ <key>CFBundleVersion</key>
+ <string>2021</string>
+ <key>LSMinimumSystemVersion</key>
+ <string>10.14</string>
+ <key>NSHighResolutionCapable</key>
+ <true/>
+</dict>
+</plist>
diff --git a/sysid/build.gradle b/sysid/build.gradle
new file mode 100644
index 0000000..e6709fc
--- /dev/null
+++ b/sysid/build.gradle
@@ -0,0 +1,172 @@
+import org.gradle.internal.os.OperatingSystem
+
+if (project.hasProperty('onlylinuxathena')) {
+ return;
+}
+
+description = 'System identification for robot mechanisms'
+
+apply plugin: 'cpp'
+apply plugin: 'google-test-test-suite'
+apply plugin: 'visual-studio'
+apply plugin: 'edu.wpi.first.NativeUtils'
+
+if (OperatingSystem.current().isWindows()) {
+ apply plugin: 'windows-resources'
+}
+
+ext {
+ nativeName = 'sysid'
+}
+
+apply from: "${rootDir}/shared/resources.gradle"
+apply from: "${rootDir}/shared/config.gradle"
+
+def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in")
+def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp")
+
+apply from: "${rootDir}/shared/imgui.gradle"
+
+task generateCppVersion() {
+ description = 'Generates the wpilib version class'
+ group = 'WPILib'
+
+ outputs.file wpilibVersionFileOutput
+ inputs.file wpilibVersionFileInput
+
+ if (wpilibVersioning.releaseMode) {
+ outputs.upToDateWhen { false }
+ }
+
+ // We follow a simple set of checks to determine whether we should generate a new version file:
+ // 1. If the release type is not development, we generate a new version file
+ // 2. If there is no generated version number, we generate a new version file
+ // 3. If there is a generated build number, and the release type is development, then we will
+ // only generate if the publish task is run.
+ doLast {
+ def version = wpilibVersioning.version.get()
+ println "Writing version ${version} to $wpilibVersionFileOutput"
+
+ if (wpilibVersionFileOutput.exists()) {
+ wpilibVersionFileOutput.delete()
+ }
+ def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version)
+ wpilibVersionFileOutput.write(read)
+ }
+}
+
+gradle.taskGraph.addTaskExecutionGraphListener { graph ->
+ def willPublish = graph.hasTask(publish)
+ if (willPublish) {
+ generateCppVersion.outputs.upToDateWhen { false }
+ }
+}
+
+def generateTask = createGenerateResourcesTask('main', 'SYSID', 'sysid', project)
+
+project(':').libraryBuild.dependsOn build
+tasks.withType(CppCompile) {
+ dependsOn generateTask
+ dependsOn generateCppVersion
+}
+
+model {
+ components {
+ // By default, a development executable will be generated. This is to help the case of
+ // testing specific functionality of the library.
+ "${nativeName}"(NativeExecutableSpec) {
+ baseName = 'sysid'
+ sources {
+ cpp {
+ source {
+ srcDirs 'src/main/native/cpp', "$buildDir/generated/main/cpp"
+ include '**/*.cpp'
+ }
+ exportedHeaders {
+ srcDirs 'src/main/native/include'
+ }
+ }
+ if (OperatingSystem.current().isWindows()) {
+ rc.source {
+ srcDirs 'src/main/native/win'
+ include '*.rc'
+ }
+ }
+ }
+ binaries.all {
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ it.buildable = false
+ return
+ }
+ lib project: ':glass', library: 'glassnt', linkage: 'static'
+ lib project: ':glass', library: 'glass', linkage: 'static'
+ project(':ntcore').addNtcoreDependency(it, 'static')
+ lib project: ':wpinet', library: 'wpinet', linkage: 'static'
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+ lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+ lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+ nativeUtils.useRequiredLibrary(it, 'imgui')
+ if (it.targetPlatform.operatingSystem.isWindows()) {
+ it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
+ it.linker.args << '/DELAYLOAD:MF.dll' << '/DELAYLOAD:MFReadWrite.dll' << '/DELAYLOAD:MFPlat.dll' << '/delay:nobind'
+ } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+ it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
+ if (it.buildType.getName() == "release") {
+ it.linker.args << '-s'
+ }
+ } else {
+ it.linker.args << '-lX11'
+ if (it.targetPlatform.name.startsWith('linuxarm')) {
+ it.linker.args << '-lGL'
+ }
+ }
+ }
+ }
+ }
+ testSuites {
+ "${nativeName}Test"(GoogleTestTestSuiteSpec) {
+ for (NativeComponentSpec c : $.components) {
+ if (c.name == nativeName) {
+ testing c
+ break
+ }
+ }
+ sources.cpp.source {
+ srcDirs "src/test/native/cpp"
+ include "**/*.cpp"
+ }
+ binaries.all {
+ if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+ it.buildable = false
+ return
+ }
+ lib project: ':glass', library: 'glassnt', linkage: 'static'
+ lib project: ':glass', library: 'glass', linkage: 'static'
+ project(':ntcore').addNtcoreDependency(it, 'static')
+ lib project: ':wpinet', library: 'wpinet', linkage: 'static'
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+ lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+ lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+ nativeUtils.useRequiredLibrary(it, 'imgui')
+ if (it.targetPlatform.operatingSystem.isWindows()) {
+ it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
+ it.linker.args << '/DELAYLOAD:MF.dll' << '/DELAYLOAD:MFReadWrite.dll' << '/DELAYLOAD:MFPlat.dll' << '/delay:nobind'
+ } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+ it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
+ if (it.buildType.getName() == "release") {
+ it.linker.args << '-s'
+ }
+ } else {
+ it.linker.args << '-lX11'
+ if (it.targetPlatform.name.startsWith('linuxarm')) {
+ it.linker.args << '-lGL'
+ }
+ }
+ nativeUtils.useRequiredLibrary(it, "googletest_static")
+ it.cppCompiler.define("RUNNING_SYSID_TESTS")
+ }
+ }
+ }
+}
+
+apply from: 'publish.gradle'
diff --git a/sysid/docs/arm-ols-with-angle-offset.md b/sysid/docs/arm-ols-with-angle-offset.md
new file mode 100644
index 0000000..ecb5c43
--- /dev/null
+++ b/sysid/docs/arm-ols-with-angle-offset.md
@@ -0,0 +1,66 @@
+# Arm OLS with angle offset
+
+If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit
+for `Kg` will be wrong. An angle offset should be added to the model like so.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset)
+```
+Use a trig identity to split the cosine into two terms.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset))
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset)
+```
+Reorder multiplicands so the offset trig is absorbed by the OLS terms.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle)
+```
+
+## OLS
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`.
+```
+dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
+```
+
+### Ks, Kv, Ka
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+```
+
+### Kg
+
+Take the sum of squares of the OLS terms containing the angle offset. The angle
+offset trig functions will form a trig identity that cancels out. Then, just
+solve for `Kg`.
+```
+δ²+ε² = (-Kg/Ka cos(offset))² + (Kg/Ka sin(offset))²
+δ²+ε² = (-Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
+δ²+ε² = (Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
+δ²+ε² = (Kg/Ka)² (cos²(offset) + sin²(offset))
+δ²+ε² = (Kg/Ka)² (1)
+δ²+ε² = (Kg/Ka)²
+√(δ²+ε²) = Kg/Ka
+√(δ²+ε²) = Kg β
+Kg = √(δ²+ε²)/β
+```
+
+As a sanity check, when the offset is zero, ε is zero and the equation for
+`Kg` simplifies to -δ/β, the equation previously used by SysId.
+
+### Angle offset
+
+Divide ε by δ, combine the trig functions into `tan(offset)`, then use `atan2()`
+to preserve the angle quadrant. Maintaining the proper negative signs in the
+numerator and denominator are important for obtaining the correct result.
+```
+δ = -Kg/Ka cos(offset)
+ε = Kg/Ka sin(offset)
+sin(offset)/-cos(offset) = ε/δ
+sin(offset)/cos(offset) = ε/-δ
+tan(offset) = ε/-δ
+offset = atan2(ε, -δ)
+```
diff --git a/sysid/docs/data-collection.md b/sysid/docs/data-collection.md
new file mode 100644
index 0000000..9d28a15
--- /dev/null
+++ b/sysid/docs/data-collection.md
@@ -0,0 +1,210 @@
+# Data Collection
+
+This document details how data must be sent over NetworkTables for accurate data collection. Note that the data format has changed from what the old [frc-characterization](https://github.com/wpilibsuite/frc-characterization) tool used to generate.
+
+## NetworkTables Data Entries
+
+Here is a list of the NT entries that are used to send and collect data between sysid and the robot program:
+
+| NT Entry | Type | Description |
+| --------------------------------------| -------- | -------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `/SmartDashboard/SysIdTelemetry` | `string` | Used to send telemetry from the robot program. This data is sent after the test completes once the robot enters the disabled state. |
+| `/SmartDashboard/SysIdVoltageCommand` | `double` | Used to either send the ramp rate (V/s) for the quasistatic test or the voltage (V) for the dynamic test. |
+| `/SmartDashboard/SysIdTestType` | `string` | Used to send the test type ("Quasistatic" or "Dynamic") which helps determine how the `VoltageCommand` entry will be used. |
+| `/SmartDashboard/SysIdRotate` | `bool` | Used to receive the rotation bool from the Logger. If this is set to true, the drivetrain will rotate. It is only applicable for drivetrain tests. |
+
+## Telemetry Format
+
+There are two formats used to send telemetry from the robot program. One format is for non-drivetrain mechanisms, whereas the other is for all drivetrain tests (linear and angular).
+
+### Non-Drivetrain Mechanisms
+
+`timestamp, voltage, position, velocity`
+
+Example JSON:
+
+```json
+{
+"fast-backward": [
+[
+timestamp 1,
+voltage 1,
+position 1,
+velocity 1
+],
+[
+timestamp 2,
+voltage 2,
+position 2,
+velocity 2
+]
+],
+"fast-forward": [
+[
+timestamp 1,
+voltage 1,
+position 1,
+velocity 1
+],
+[
+timestamp 2,
+voltage 2,
+position 2,
+velocity 2
+]
+],
+"slow-backward": [
+[
+timestamp 1,
+voltage 1,
+position 1,
+velocity 1
+],
+[
+timestamp 2,
+voltage 2,
+position 2,
+velocity 2
+]
+],
+"slow-forward": [
+[
+timestamp 1,
+voltage 1,
+position 1,
+velocity 1
+],
+[
+timestamp 2,
+voltage 2,
+position 2,
+velocity 2
+]
+],
+"sysid": true,
+"test": "Simple",
+"units": "Rotations",
+"unitsPerRotation": 1.0
+}
+```
+
+Supported test types for the "test" field in this data format include "Arm",
+"Elevator", and "Simple". Supported unit types include "Meters", "Feet",
+"Inches", "Radians", "Rotations", and "Degrees".
+
+### Drivetrain
+
+`timestamp, l voltage, r voltage, l position, r position, l velocity, r velocity, angle, angular rate`
+
+Note that all positions and velocities should be in rotations of the output and rotations/sec of the output respectively. If there is a gearing between the encoder and the output, that should be taken into account.
+
+Example JSON:
+
+```json
+{
+"fast-backward": [
+[
+timestamp 1,
+l voltage 1,
+r voltage 1,
+l position 1,
+r position 1,
+l velocity 1,
+r velocity 1,
+angle 1,
+angular rate 1
+],
+[
+timestamp 2,
+l voltage 2,
+r voltage 2,
+l position 2,
+r position 2,
+l velocity 2,
+r velocity 2,
+angle 2,
+angular rate 2
+]
+],
+"fast-forward": [
+[
+timestamp 1,
+l voltage 1,
+r voltage 1,
+l position 1,
+r position 1,
+l velocity 1,
+r velocity 1,
+angle 1,
+angular rate 1
+],
+[
+timestamp 2,
+l voltage 2,
+r voltage 2,
+l position 2,
+r position 2,
+l velocity 2,
+r velocity 2,
+angle 2,
+angular rate 2
+]
+],
+"slow-backward": [
+[
+timestamp 1,
+l voltage 1,
+r voltage 1,
+l position 1,
+r position 1,
+l velocity 1,
+r velocity 1,
+angle 1,
+angular rate 1
+],
+[
+timestamp 2,
+l voltage 2,
+r voltage 2,
+l position 2,
+r position 2,
+l velocity 2,
+r velocity 2,
+angle 2,
+angular rate 2
+]
+],
+"slow-forward": [
+[
+timestamp 1,
+l voltage 1,
+r voltage 1,
+l position 1,
+r position 1,
+l velocity 1,
+r velocity 1,
+angle 1,
+angular rate 1
+],
+[
+timestamp 2,
+l voltage 2,
+r voltage 2,
+l position 2,
+r position 2,
+l velocity 2,
+r velocity 2,
+angle 2,
+angular rate 2
+]
+],
+"sysid": true,
+"test": "Drivetrain",
+"units": "Rotations",
+"unitsPerRotation": 1.0
+}
+```
+
+Supported test types for the "test" field in this data format include
+"Drivetrain" and "Drivetrain (Angular)". Supported unit types include "Meters",
+"Feet", "Inches", "Radians", "Rotations", and "Degrees".
diff --git a/sysid/publish.gradle b/sysid/publish.gradle
new file mode 100644
index 0000000..1356946
--- /dev/null
+++ b/sysid/publish.gradle
@@ -0,0 +1,125 @@
+apply plugin: 'maven-publish'
+
+def baseArtifactId = 'SysId'
+def artifactGroupId = 'edu.wpi.first.tools'
+def zipBaseName = '_GROUP_edu_wpi_first_tools_ID_SysId_CLS'
+
+def outputsFolder = file("$project.buildDir/outputs")
+
+model {
+ tasks {
+ // Create the run task.
+ $.components.sysid.binaries.each { bin ->
+ if (bin.buildable && bin.name.toLowerCase().contains("debug") && nativeUtils.isNativeDesktopPlatform(bin.targetPlatform)) {
+ Task run = project.tasks.create("run", Exec) {
+ commandLine bin.tasks.install.runScriptFile.get().asFile.toString()
+ }
+ run.dependsOn bin.tasks.install
+ }
+ }
+ }
+ publishing {
+ def sysIdTaskList = []
+ $.components.each { component ->
+ component.binaries.each { binary ->
+ if (binary in NativeExecutableBinarySpec && binary.component.name.contains("sysid")) {
+ if (binary.buildable && (binary.name.contains('Release') || binary.name.contains('release'))) {
+ // We are now in the binary that we want.
+ // This is the default application path for the ZIP task.
+ def applicationPath = binary.executable.file
+ def icon = file("$project.projectDir/src/main/native/mac/ov.icns")
+
+ // Create the macOS bundle.
+ def bundleTask = project.tasks.create("bundleSysIdOsxApp" + binary.targetPlatform.architecture.name, Copy) {
+ description("Creates a macOS application bundle for SysId")
+ from(file("$project.projectDir/Info.plist"))
+ into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/SysId.app/Contents"))
+ into("MacOS") {
+ with copySpec {
+ from binary.executable.file
+ }
+ }
+ into("Resources") {
+ with copySpec {
+ from icon
+ }
+ }
+
+ inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
+ doLast {
+ if (project.hasProperty("developerID")) {
+ // Get path to binary.
+ exec {
+ workingDir rootDir
+ def args = [
+ "sh",
+ "-c",
+ "codesign --force --strict --deep " +
+ "--timestamp --options=runtime " +
+ "--verbose -s ${project.findProperty("developerID")} " +
+ "$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/SysId.app/"
+ ]
+ commandLine args
+ }
+ }
+ }
+ }
+
+ // Reset the application path if we are creating a bundle.
+ if (binary.targetPlatform.operatingSystem.isMacOsX()) {
+ applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name")
+ project.build.dependsOn bundleTask
+ }
+
+ // Create the ZIP.
+ def task = project.tasks.create("copySysIdExecutable" + binary.targetPlatform.architecture.name, Zip) {
+ description("Copies the SysId executable to the outputs directory.")
+ destinationDirectory = outputsFolder
+
+ archiveBaseName = '_M_' + zipBaseName
+ duplicatesStrategy = 'exclude'
+ archiveClassifier = nativeUtils.getPublishClassifier(binary)
+
+ from(licenseFile) {
+ into '/'
+ }
+
+ from(applicationPath)
+
+ if (binary.targetPlatform.operatingSystem.isWindows()) {
+ def exePath = binary.executable.file.absolutePath
+ exePath = exePath.substring(0, exePath.length() - 4)
+ def pdbPath = new File(exePath + '.pdb')
+ from(pdbPath)
+ }
+
+ into(nativeUtils.getPlatformPath(binary))
+ }
+
+ if (binary.targetPlatform.operatingSystem.isMacOsX()) {
+ bundleTask.dependsOn binary.tasks.link
+ task.dependsOn(bundleTask)
+ }
+
+ task.dependsOn binary.tasks.link
+ sysIdTaskList.add(task)
+ project.build.dependsOn task
+ project.artifacts { task }
+ addTaskToCopyAllOutputs(task)
+ }
+ }
+ }
+ }
+
+ publications {
+ sysId(MavenPublication) {
+ sysIdTaskList.each { artifact it }
+
+ artifactId = baseArtifactId
+ groupId = artifactGroupId
+ version wpilibVersioning.version.get()
+ }
+ }
+ }
+}
diff --git a/sysid/scripts/time_plots.py b/sysid/scripts/time_plots.py
new file mode 100755
index 0000000..6878126
--- /dev/null
+++ b/sysid/scripts/time_plots.py
@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+
+import json
+import pathlib
+
+import matplotlib.pyplot as plt
+import pandas as pd
+import sys
+
+# Load data
+filename = pathlib.Path(sys.argv[1])
+
+UNIT_TO_ABBREVIATION = {
+ "Meters": "m",
+ "Feet": "ft",
+ "Inches": "in",
+ "Degrees": "deg",
+ "Rotations": "rot",
+ "Radians": "rad",
+}
+
+# Make DataFrame to facilitate plotting
+if filename.suffix == ".json":
+ raw_data = json.loads(filename.read_text())
+ unit = raw_data["units"]
+
+ # Get Unit
+ try:
+ abbreviation = UNIT_TO_ABBREVIATION[unit]
+ except KeyError:
+ raise ValueError("Invalid Unit")
+
+ # Make Columns
+ columns = ["Timestamp (s)", "Test"]
+ if "Drive" in raw_data["test"]:
+ columns.extend(
+ [
+ "Left Volts (V)",
+ "Right Volts (V)",
+ f"Left Position ({abbreviation})",
+ f"Right Position ({abbreviation})",
+ f"Left Velocity ({abbreviation}/s)",
+ f"Right Velocity ({abbreviation}/s)",
+ "Gyro Position (deg)",
+ "Gyro Rate (deg/s)",
+ ]
+ )
+ unit_columns = columns[4:8]
+ else:
+ columns.extend(
+ ["Volts (V)", f"Position ({abbreviation})", f"Velocity ({abbreviation}/s)"]
+ )
+ unit_columns = columns[3:]
+
+ prepared_data = pd.DataFrame(columns=columns)
+ for test in raw_data.keys():
+ if "-" not in test:
+ continue
+ formatted_entry = [[pt[0], test, *pt[1:]] for pt in raw_data[test]]
+ prepared_data = pd.concat(
+ [prepared_data, pd.DataFrame(formatted_entry, columns=columns)]
+ )
+
+ units_per_rot = raw_data["unitsPerRotation"]
+
+ for column in unit_columns:
+ prepared_data[column] *= units_per_rot
+else:
+ prepared_data = pd.read_csv(filename)
+
+# First 2 columns are Timestamp and Test
+for column in prepared_data.columns[2:]:
+ # Configure Plot Labels
+ plt.figure()
+ plt.xlabel("Timestamp (s)")
+ plt.ylabel(column)
+
+ # Configure title without units
+ print(column)
+ end = column.find("(")
+ plt.title(f"{column[:end].strip()} vs Time")
+
+ # Plot data for each test
+ for test in pd.unique(prepared_data["Test"]):
+ test_data = prepared_data[prepared_data["Test"] == test]
+ plt.plot(test_data["Timestamp (s)"], test_data[column], label=test)
+ plt.legend()
+
+plt.show()
diff --git a/sysid/src/main/generate/WPILibVersion.cpp.in b/sysid/src/main/generate/WPILibVersion.cpp.in
new file mode 100644
index 0000000..b0a4490
--- /dev/null
+++ b/sysid/src/main/generate/WPILibVersion.cpp.in
@@ -0,0 +1,7 @@
+/*
+ * Autogenerated file! Do not manually edit this file. This version is regenerated
+ * any time the publish task is run, or when this file is deleted.
+ */
+const char* GetWPILibVersion() {
+ return "${wpilib_version}";
+}
diff --git a/sysid/src/main/native/cpp/App.cpp b/sysid/src/main/native/cpp/App.cpp
new file mode 100644
index 0000000..947ea43
--- /dev/null
+++ b/sysid/src/main/native/cpp/App.cpp
@@ -0,0 +1,219 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cstdio>
+
+#ifndef RUNNING_SYSID_TESTS
+
+#include <filesystem>
+#include <memory>
+#include <string_view>
+
+#include <fmt/format.h>
+#include <glass/Context.h>
+#include <glass/MainMenuBar.h>
+#include <glass/Storage.h>
+#include <glass/Window.h>
+#include <glass/WindowManager.h>
+#include <glass/other/Log.h>
+#include <imgui.h>
+#include <uv.h>
+#include <wpi/Logger.h>
+#include <wpigui.h>
+#include <wpigui_openurl.h>
+
+#include "sysid/view/Analyzer.h"
+#include "sysid/view/JSONConverter.h"
+#include "sysid/view/Logger.h"
+#include "sysid/view/UILayout.h"
+
+namespace gui = wpi::gui;
+
+static std::unique_ptr<glass::WindowManager> gWindowManager;
+
+glass::Window* gLoggerWindow;
+glass::Window* gAnalyzerWindow;
+glass::Window* gProgramLogWindow;
+static glass::MainMenuBar gMainMenu;
+
+std::unique_ptr<sysid::JSONConverter> gJSONConverter;
+
+glass::LogData gLog;
+wpi::Logger gLogger;
+
+const char* GetWPILibVersion();
+
+namespace sysid {
+std::string_view GetResource_sysid_16_png();
+std::string_view GetResource_sysid_32_png();
+std::string_view GetResource_sysid_48_png();
+std::string_view GetResource_sysid_64_png();
+std::string_view GetResource_sysid_128_png();
+std::string_view GetResource_sysid_256_png();
+std::string_view GetResource_sysid_512_png();
+} // namespace sysid
+
+void Application(std::string_view saveDir) {
+ // Create the wpigui (along with Dear ImGui) and Glass contexts.
+ gui::CreateContext();
+ glass::CreateContext();
+
+ // Add icons
+ gui::AddIcon(sysid::GetResource_sysid_16_png());
+ gui::AddIcon(sysid::GetResource_sysid_32_png());
+ gui::AddIcon(sysid::GetResource_sysid_48_png());
+ gui::AddIcon(sysid::GetResource_sysid_64_png());
+ gui::AddIcon(sysid::GetResource_sysid_128_png());
+ gui::AddIcon(sysid::GetResource_sysid_256_png());
+ gui::AddIcon(sysid::GetResource_sysid_512_png());
+
+ glass::SetStorageName("sysid");
+ glass::SetStorageDir(saveDir.empty() ? gui::GetPlatformSaveFileDir()
+ : saveDir);
+
+ // Add messages from the global sysid logger into the Log window.
+ gLogger.SetLogger([](unsigned int level, const char* file, unsigned int line,
+ const char* msg) {
+ const char* lvl = "";
+ if (level >= wpi::WPI_LOG_CRITICAL) {
+ lvl = "CRITICAL: ";
+ } else if (level >= wpi::WPI_LOG_ERROR) {
+ lvl = "ERROR: ";
+ } else if (level >= wpi::WPI_LOG_WARNING) {
+ lvl = "WARNING: ";
+ } else if (level >= wpi::WPI_LOG_INFO) {
+ lvl = "INFO: ";
+ } else if (level >= wpi::WPI_LOG_DEBUG) {
+ lvl = "DEBUG: ";
+ }
+ std::string filename = std::filesystem::path{file}.filename().string();
+ gLog.Append(fmt::format("{}{} ({}:{})\n", lvl, msg, filename, line));
+#ifndef NDEBUG
+ fmt::print(stderr, "{}{} ({}:{})\n", lvl, msg, filename, line);
+#endif
+ });
+
+ gLogger.set_min_level(wpi::WPI_LOG_DEBUG);
+ // Set the number of workers for the libuv threadpool.
+ uv_os_setenv("UV_THREADPOOL_SIZE", "6");
+
+ // Initialize window manager and add views.
+ auto& storage = glass::GetStorageRoot().GetChild("SysId");
+ gWindowManager = std::make_unique<glass::WindowManager>(storage);
+ gWindowManager->GlobalInit();
+
+ gLoggerWindow = gWindowManager->AddWindow(
+ "Logger", std::make_unique<sysid::Logger>(storage, gLogger));
+
+ gAnalyzerWindow = gWindowManager->AddWindow(
+ "Analyzer", std::make_unique<sysid::Analyzer>(storage, gLogger));
+
+ gProgramLogWindow = gWindowManager->AddWindow(
+ "Program Log", std::make_unique<glass::LogView>(&gLog));
+
+ // Set default positions and sizes for windows.
+
+ // Logger window position/size
+ gLoggerWindow->SetDefaultPos(sysid::kLoggerWindowPos.x,
+ sysid::kLoggerWindowPos.y);
+ gLoggerWindow->SetDefaultSize(sysid::kLoggerWindowSize.x,
+ sysid::kLoggerWindowSize.y);
+
+ // Analyzer window position/size
+ gAnalyzerWindow->SetDefaultPos(sysid::kAnalyzerWindowPos.x,
+ sysid::kAnalyzerWindowPos.y);
+ gAnalyzerWindow->SetDefaultSize(sysid::kAnalyzerWindowSize.x,
+ sysid::kAnalyzerWindowSize.y);
+
+ // Program log window position/size
+ gProgramLogWindow->SetDefaultPos(sysid::kProgramLogWindowPos.x,
+ sysid::kProgramLogWindowPos.y);
+ gProgramLogWindow->SetDefaultSize(sysid::kProgramLogWindowSize.x,
+ sysid::kProgramLogWindowSize.y);
+ gProgramLogWindow->DisableRenamePopup();
+
+ gJSONConverter = std::make_unique<sysid::JSONConverter>(gLogger);
+
+ // Configure save file.
+ gui::ConfigurePlatformSaveFile("sysid.ini");
+
+ // Add menu bar.
+ gui::AddLateExecute([] {
+ ImGui::BeginMainMenuBar();
+ gMainMenu.WorkspaceMenu();
+ gui::EmitViewMenu();
+
+ if (ImGui::BeginMenu("Widgets")) {
+ gWindowManager->DisplayMenu();
+ ImGui::EndMenu();
+ }
+
+ bool about = false;
+ if (ImGui::BeginMenu("Info")) {
+ if (ImGui::MenuItem("About")) {
+ about = true;
+ }
+ ImGui::EndMenu();
+ }
+
+ bool toCSV = false;
+ if (ImGui::BeginMenu("JSON Converters")) {
+ if (ImGui::MenuItem("JSON to CSV Converter")) {
+ toCSV = true;
+ }
+
+ ImGui::EndMenu();
+ }
+
+ if (ImGui::BeginMenu("Docs")) {
+ if (ImGui::MenuItem("Online documentation")) {
+ wpi::gui::OpenURL(
+ "https://docs.wpilib.org/en/stable/docs/software/pathplanning/"
+ "system-identification/");
+ }
+
+ ImGui::EndMenu();
+ }
+
+ ImGui::EndMainMenuBar();
+
+ if (toCSV) {
+ ImGui::OpenPopup("SysId JSON to CSV Converter");
+ toCSV = false;
+ }
+
+ if (ImGui::BeginPopupModal("SysId JSON to CSV Converter")) {
+ gJSONConverter->DisplayCSVConvert();
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ }
+ ImGui::EndPopup();
+ }
+
+ if (about) {
+ ImGui::OpenPopup("About");
+ about = false;
+ }
+ if (ImGui::BeginPopupModal("About")) {
+ ImGui::Text("SysId: System Identification for Robot Mechanisms");
+ ImGui::Separator();
+ ImGui::Text("v%s", GetWPILibVersion());
+ ImGui::Separator();
+ ImGui::Text("Save location: %s", glass::GetStorageDir().c_str());
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ }
+ ImGui::EndPopup();
+ }
+ });
+
+ gui::Initialize("System Identification", sysid::kAppWindowSize.x,
+ sysid::kAppWindowSize.y);
+ gui::Main();
+
+ glass::DestroyContext();
+ gui::DestroyContext();
+}
+
+#endif
diff --git a/sysid/src/main/native/cpp/Main.cpp b/sysid/src/main/native/cpp/Main.cpp
new file mode 100644
index 0000000..6fba848
--- /dev/null
+++ b/sysid/src/main/native/cpp/Main.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <string_view>
+
+#ifndef RUNNING_SYSID_TESTS
+
+void Application(std::string_view saveDir);
+
+#ifdef _WIN32
+int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine,
+ int nCmdShow) {
+ int argc = __argc;
+ char** argv = __argv;
+#else
+int main(int argc, char** argv) {
+#endif
+ std::string_view saveDir;
+ if (argc == 2) {
+ saveDir = argv[1];
+ }
+
+ Application(saveDir);
+
+ return 0;
+}
+
+#endif
diff --git a/sysid/src/main/native/cpp/Util.cpp b/sysid/src/main/native/cpp/Util.cpp
new file mode 100644
index 0000000..a9acc53
--- /dev/null
+++ b/sysid/src/main/native/cpp/Util.cpp
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/Util.h"
+
+#include <filesystem>
+#include <stdexcept>
+
+#include <imgui.h>
+#include <wpi/raw_ostream.h>
+
+void sysid::CreateTooltip(const char* text) {
+ ImGui::SameLine();
+ ImGui::TextDisabled(" (?)");
+
+ if (ImGui::IsItemHovered()) {
+ ImGui::BeginTooltip();
+ ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f);
+ ImGui::TextUnformatted(text);
+ ImGui::PopTextWrapPos();
+ ImGui::EndTooltip();
+ }
+}
+
+void sysid::CreateErrorPopup(bool& isError, std::string_view errorMessage) {
+ if (isError) {
+ ImGui::OpenPopup("Exception Caught!");
+ }
+
+ // Handle exceptions.
+ ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
+ if (ImGui::BeginPopupModal("Exception Caught!")) {
+ ImGui::PushTextWrapPos(0.0f);
+ ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s",
+ errorMessage.data());
+ ImGui::PopTextWrapPos();
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ isError = false;
+ }
+ ImGui::EndPopup();
+ }
+}
+
+std::string_view sysid::GetAbbreviation(std::string_view unit) {
+ if (unit == "Meters") {
+ return "m";
+ } else if (unit == "Feet") {
+ return "ft";
+ } else if (unit == "Inches") {
+ return "in";
+ } else if (unit == "Radians") {
+ return "rad";
+ } else if (unit == "Degrees") {
+ return "deg";
+ } else if (unit == "Rotations") {
+ return "rot";
+ } else {
+ throw std::runtime_error("Invalid Unit");
+ }
+}
+
+void sysid::SaveFile(std::string_view contents,
+ const std::filesystem::path& path) {
+ // Create the path if it doesn't already exist.
+ std::filesystem::create_directories(path.root_directory());
+
+ // Open a fd_ostream to write to file.
+ std::error_code ec;
+ wpi::raw_fd_ostream ostream{path.string(), ec};
+
+ // Check error code.
+ if (ec) {
+ throw std::runtime_error("Cannot write to file: " + ec.message());
+ }
+
+ // Write contents.
+ ostream << contents;
+}
diff --git a/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
new file mode 100644
index 0000000..b15cae8
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
@@ -0,0 +1,568 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/AnalysisManager.h"
+
+#include <cmath>
+#include <cstddef>
+#include <functional>
+#include <stdexcept>
+
+#include <fmt/format.h>
+#include <units/angle.h>
+#include <units/math.h>
+#include <wpi/MemoryBuffer.h>
+#include <wpi/StringExtras.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/analysis/JSONConverter.h"
+#include "sysid/analysis/TrackWidthAnalysis.h"
+
+using namespace sysid;
+
+/**
+ * Converts a raw data vector into a PreparedData vector with only the
+ * timestamp, voltage, position, and velocity fields filled out.
+ *
+ * @tparam S The size of the arrays in the raw data vector
+ * @tparam Timestamp The index of the Timestamp data in the raw data vector
+ * arrays
+ * @tparam Voltage The index of the Voltage data in the raw data vector arrays
+ * @tparam Position The index of the Position data in the raw data vector arrays
+ * @tparam Velocity The index of the Velocity data in the raw data vector arrays
+ *
+ * @param data A raw data vector
+ *
+ * @return A PreparedData vector
+ */
+template <size_t S, size_t Timestamp, size_t Voltage, size_t Position,
+ size_t Velocity>
+static std::vector<PreparedData> ConvertToPrepared(
+ const std::vector<std::array<double, S>>& data) {
+ std::vector<PreparedData> prepared;
+ for (int i = 0; i < static_cast<int>(data.size()) - 1; ++i) {
+ const auto& pt1 = data[i];
+ const auto& pt2 = data[i + 1];
+ prepared.emplace_back(PreparedData{
+ units::second_t{pt1[Timestamp]}, pt1[Voltage], pt1[Position],
+ pt1[Velocity], units::second_t{pt2[Timestamp] - pt1[Timestamp]}});
+ }
+ return prepared;
+}
+
+/**
+ * To preserve a raw copy of the data, this method saves a raw version
+ * in the dataset StringMap where the key of the raw data starts with "raw-"
+ * before the name of the original data.
+ *
+ * @tparam S The size of the array data that's being used
+ *
+ * @param dataset A reference to the dataset being used
+ */
+template <size_t S>
+static void CopyRawData(
+ wpi::StringMap<std::vector<std::array<double, S>>>* dataset) {
+ auto& data = *dataset;
+ // Loads the Raw Data
+ for (auto& it : data) {
+ auto key = it.first();
+ auto& dataset = it.getValue();
+
+ if (!wpi::contains(key, "raw")) {
+ data[fmt::format("raw-{}", key)] = dataset;
+ data[fmt::format("original-raw-{}", key)] = dataset;
+ }
+ }
+}
+
+/**
+ * Assigns the combines the various datasets into a single one for analysis.
+ *
+ * @param slowForward The slow forward dataset
+ * @param slowBackward The slow backward dataset
+ * @param fastForward The fast forward dataset
+ * @param fastBackward The fast backward dataset
+ */
+static Storage CombineDatasets(const std::vector<PreparedData>& slowForward,
+ const std::vector<PreparedData>& slowBackward,
+ const std::vector<PreparedData>& fastForward,
+ const std::vector<PreparedData>& fastBackward) {
+ return Storage{slowForward, slowBackward, fastForward, fastBackward};
+}
+
+void AnalysisManager::PrepareGeneralData() {
+ using Data = std::array<double, 4>;
+ wpi::StringMap<std::vector<Data>> data;
+ wpi::StringMap<std::vector<PreparedData>> preparedData;
+
+ // Store the raw data columns.
+ static constexpr size_t kTimeCol = 0;
+ static constexpr size_t kVoltageCol = 1;
+ static constexpr size_t kPosCol = 2;
+ static constexpr size_t kVelCol = 3;
+
+ WPI_INFO(m_logger, "{}", "Reading JSON data.");
+ // Get the major components from the JSON and store them inside a StringMap.
+ for (auto&& key : AnalysisManager::kJsonDataKeys) {
+ data[key] = m_json.at(key).get<std::vector<Data>>();
+ }
+
+ WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
+ // Ensure that voltage and velocity have the same sign. Also multiply
+ // positions and velocities by the factor.
+ for (auto it = data.begin(); it != data.end(); ++it) {
+ for (auto&& pt : it->second) {
+ pt[kVoltageCol] = std::copysign(pt[kVoltageCol], pt[kVelCol]);
+ pt[kPosCol] *= m_factor;
+ pt[kVelCol] *= m_factor;
+ }
+ }
+
+ WPI_INFO(m_logger, "{}", "Copying raw data.");
+ CopyRawData(&data);
+
+ WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct.");
+ // Convert data to PreparedData structs
+ for (auto& it : data) {
+ auto key = it.first();
+ preparedData[key] =
+ ConvertToPrepared<4, kTimeCol, kVoltageCol, kPosCol, kVelCol>(
+ data[key]);
+ }
+
+ // Store the original datasets
+ m_originalDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(preparedData["original-raw-slow-forward"],
+ preparedData["original-raw-slow-backward"],
+ preparedData["original-raw-fast-forward"],
+ preparedData["original-raw-fast-backward"]);
+
+ WPI_INFO(m_logger, "{}", "Initial trimming and filtering.");
+ sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
+ m_velocityDelays, m_minStepTime, m_maxStepTime,
+ m_unit);
+
+ WPI_INFO(m_logger, "{}", "Acceleration filtering.");
+ sysid::AccelFilter(&preparedData);
+
+ WPI_INFO(m_logger, "{}", "Storing datasets.");
+ // Store the raw datasets
+ m_rawDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(
+ preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
+ preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
+
+ // Store the filtered datasets
+ m_filteredDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(
+ preparedData["slow-forward"], preparedData["slow-backward"],
+ preparedData["fast-forward"], preparedData["fast-backward"]);
+
+ m_startTimes = {preparedData["raw-slow-forward"][0].timestamp,
+ preparedData["raw-slow-backward"][0].timestamp,
+ preparedData["raw-fast-forward"][0].timestamp,
+ preparedData["raw-fast-backward"][0].timestamp};
+}
+
+void AnalysisManager::PrepareAngularDrivetrainData() {
+ using Data = std::array<double, 9>;
+ wpi::StringMap<std::vector<Data>> data;
+ wpi::StringMap<std::vector<PreparedData>> preparedData;
+
+ // Store the relevant raw data columns.
+ static constexpr size_t kTimeCol = 0;
+ static constexpr size_t kLVoltageCol = 1;
+ static constexpr size_t kRVoltageCol = 2;
+ static constexpr size_t kLPosCol = 3;
+ static constexpr size_t kRPosCol = 4;
+ static constexpr size_t kLVelCol = 5;
+ static constexpr size_t kRVelCol = 6;
+ static constexpr size_t kAngleCol = 7;
+ static constexpr size_t kAngularRateCol = 8;
+
+ WPI_INFO(m_logger, "{}", "Reading JSON data.");
+ // Get the major components from the JSON and store them inside a StringMap.
+ for (auto&& key : AnalysisManager::kJsonDataKeys) {
+ data[key] = m_json.at(key).get<std::vector<Data>>();
+ }
+
+ WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
+ // Ensure that voltage and velocity have the same sign. Also multiply
+ // positions and velocities by the factor.
+ for (auto it = data.begin(); it != data.end(); ++it) {
+ for (auto&& pt : it->second) {
+ pt[kLPosCol] *= m_factor;
+ pt[kRPosCol] *= m_factor;
+ pt[kLVelCol] *= m_factor;
+ pt[kRVelCol] *= m_factor;
+
+ // Stores the average voltages in the left voltage column.
+ // This aggregates the left and right voltages into a single voltage
+ // column for the ConvertToPrepared() method. std::copysign() ensures the
+ // polarity of the voltage matches the direction the robot turns.
+ pt[kLVoltageCol] = std::copysign(
+ (std::abs(pt[kLVoltageCol]) + std::abs(pt[kRVoltageCol])) / 2,
+ pt[kAngularRateCol]);
+
+ // ω = (v_r - v_l) / trackwidth
+ // v = ωr => v = ω * trackwidth / 2
+ // (v_r - v_l) / trackwidth * (trackwidth / 2) = (v_r - v_l) / 2
+ // However, since we know this is an angular test, the left and right
+ // wheel velocities will have opposite signs, allowing us to add their
+ // absolute values and get the same result (in terms of magnitude).
+ // std::copysign() is used to make sure the direction of the wheel
+ // velocities matches the direction the robot turns.
+ pt[kAngularRateCol] =
+ std::copysign((std::abs(pt[kRVelCol]) + std::abs(pt[kLVelCol])) / 2,
+ pt[kAngularRateCol]);
+ }
+ }
+
+ WPI_INFO(m_logger, "{}", "Calculating trackwidth");
+ // Aggregating all the deltas from all the tests
+ double leftDelta = 0.0;
+ double rightDelta = 0.0;
+ double angleDelta = 0.0;
+ for (const auto& it : data) {
+ auto key = it.first();
+ auto& trackWidthData = data[key];
+ leftDelta += std::abs(trackWidthData.back()[kLPosCol] -
+ trackWidthData.front()[kLPosCol]);
+ rightDelta += std::abs(trackWidthData.back()[kRPosCol] -
+ trackWidthData.front()[kRPosCol]);
+ angleDelta += std::abs(trackWidthData.back()[kAngleCol] -
+ trackWidthData.front()[kAngleCol]);
+ }
+ m_trackWidth = sysid::CalculateTrackWidth(leftDelta, rightDelta,
+ units::radian_t{angleDelta});
+
+ WPI_INFO(m_logger, "{}", "Copying raw data.");
+ CopyRawData(&data);
+
+ WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
+ // Convert raw data to prepared data
+ for (const auto& it : data) {
+ auto key = it.first();
+ preparedData[key] = ConvertToPrepared<9, kTimeCol, kLVoltageCol, kAngleCol,
+ kAngularRateCol>(data[key]);
+ }
+
+ // Create the distinct datasets and store them
+ m_originalDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(preparedData["original-raw-slow-forward"],
+ preparedData["original-raw-slow-backward"],
+ preparedData["original-raw-fast-forward"],
+ preparedData["original-raw-fast-backward"]);
+
+ WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
+ sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
+ m_velocityDelays, m_minStepTime, m_maxStepTime);
+
+ WPI_INFO(m_logger, "{}", "Acceleration filtering.");
+ sysid::AccelFilter(&preparedData);
+
+ WPI_INFO(m_logger, "{}", "Storing datasets.");
+ // Create the distinct datasets and store them
+ m_rawDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(
+ preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
+ preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
+ m_filteredDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(
+ preparedData["slow-forward"], preparedData["slow-backward"],
+ preparedData["fast-forward"], preparedData["fast-backward"]);
+
+ m_startTimes = {preparedData["slow-forward"][0].timestamp,
+ preparedData["slow-backward"][0].timestamp,
+ preparedData["fast-forward"][0].timestamp,
+ preparedData["fast-backward"][0].timestamp};
+}
+
+void AnalysisManager::PrepareLinearDrivetrainData() {
+ using Data = std::array<double, 9>;
+ wpi::StringMap<std::vector<Data>> data;
+ wpi::StringMap<std::vector<PreparedData>> preparedData;
+
+ // Store the relevant raw data columns.
+ static constexpr size_t kTimeCol = 0;
+ static constexpr size_t kLVoltageCol = 1;
+ static constexpr size_t kRVoltageCol = 2;
+ static constexpr size_t kLPosCol = 3;
+ static constexpr size_t kRPosCol = 4;
+ static constexpr size_t kLVelCol = 5;
+ static constexpr size_t kRVelCol = 6;
+
+ // Get the major components from the JSON and store them inside a StringMap.
+ WPI_INFO(m_logger, "{}", "Reading JSON data.");
+ for (auto&& key : AnalysisManager::kJsonDataKeys) {
+ data[key] = m_json.at(key).get<std::vector<Data>>();
+ }
+
+ // Ensure that voltage and velocity have the same sign. Also multiply
+ // positions and velocities by the factor.
+ WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
+ for (auto it = data.begin(); it != data.end(); ++it) {
+ for (auto&& pt : it->second) {
+ pt[kLVoltageCol] = std::copysign(pt[kLVoltageCol], pt[kLVelCol]);
+ pt[kRVoltageCol] = std::copysign(pt[kRVoltageCol], pt[kRVelCol]);
+ pt[kLPosCol] *= m_factor;
+ pt[kRPosCol] *= m_factor;
+ pt[kLVelCol] *= m_factor;
+ pt[kRVelCol] *= m_factor;
+ }
+ }
+
+ WPI_INFO(m_logger, "{}", "Copying raw data.");
+ CopyRawData(&data);
+
+ // Convert data to PreparedData
+ WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
+ for (auto& it : data) {
+ auto key = it.first();
+
+ preparedData[fmt::format("left-{}", key)] =
+ ConvertToPrepared<9, kTimeCol, kLVoltageCol, kLPosCol, kLVelCol>(
+ data[key]);
+ preparedData[fmt::format("right-{}", key)] =
+ ConvertToPrepared<9, kTimeCol, kRVoltageCol, kRPosCol, kRVelCol>(
+ data[key]);
+ }
+
+ // Create the distinct raw datasets and store them
+ auto originalSlowForward = AnalysisManager::DataConcat(
+ preparedData["left-original-raw-slow-forward"],
+ preparedData["right-original-raw-slow-forward"]);
+ auto originalSlowBackward = AnalysisManager::DataConcat(
+ preparedData["left-original-raw-slow-backward"],
+ preparedData["right-original-raw-slow-backward"]);
+ auto originalFastForward = AnalysisManager::DataConcat(
+ preparedData["left-original-raw-fast-forward"],
+ preparedData["right-original-raw-fast-forward"]);
+ auto originalFastBackward = AnalysisManager::DataConcat(
+ preparedData["left-original-raw-fast-backward"],
+ preparedData["right-original-raw-fast-backward"]);
+ m_originalDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(originalSlowForward, originalSlowBackward,
+ originalFastForward, originalFastBackward);
+ m_originalDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
+ CombineDatasets(preparedData["left-original-raw-slow-forward"],
+ preparedData["left-original-raw-slow-backward"],
+ preparedData["left-original-raw-fast-forward"],
+ preparedData["left-original-raw-fast-backward"]);
+ m_originalDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kRight)] =
+ CombineDatasets(preparedData["right-original-raw-slow-forward"],
+ preparedData["right-original-raw-slow-backward"],
+ preparedData["right-original-raw-fast-forward"],
+ preparedData["right-original-raw-fast-backward"]);
+
+ WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
+ sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
+ m_velocityDelays, m_minStepTime, m_maxStepTime);
+
+ auto slowForward = AnalysisManager::DataConcat(
+ preparedData["left-slow-forward"], preparedData["right-slow-forward"]);
+ auto slowBackward = AnalysisManager::DataConcat(
+ preparedData["left-slow-backward"], preparedData["right-slow-backward"]);
+ auto fastForward = AnalysisManager::DataConcat(
+ preparedData["left-fast-forward"], preparedData["right-fast-forward"]);
+ auto fastBackward = AnalysisManager::DataConcat(
+ preparedData["left-fast-backward"], preparedData["right-fast-backward"]);
+
+ WPI_INFO(m_logger, "{}", "Acceleration filtering.");
+ sysid::AccelFilter(&preparedData);
+
+ WPI_INFO(m_logger, "{}", "Storing datasets.");
+
+ // Create the distinct raw datasets and store them
+ auto rawSlowForward =
+ AnalysisManager::DataConcat(preparedData["left-raw-slow-forward"],
+ preparedData["right-raw-slow-forward"]);
+ auto rawSlowBackward =
+ AnalysisManager::DataConcat(preparedData["left-raw-slow-backward"],
+ preparedData["right-raw-slow-backward"]);
+ auto rawFastForward =
+ AnalysisManager::DataConcat(preparedData["left-raw-fast-forward"],
+ preparedData["right-raw-fast-forward"]);
+ auto rawFastBackward =
+ AnalysisManager::DataConcat(preparedData["left-raw-fast-backward"],
+ preparedData["right-raw-fast-backward"]);
+
+ m_rawDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward,
+ rawFastBackward);
+ m_rawDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
+ CombineDatasets(preparedData["left-raw-slow-forward"],
+ preparedData["left-raw-slow-backward"],
+ preparedData["left-raw-fast-forward"],
+ preparedData["left-raw-fast-backward"]);
+ m_rawDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kRight)] =
+ CombineDatasets(preparedData["right-raw-slow-forward"],
+ preparedData["right-raw-slow-backward"],
+ preparedData["right-raw-fast-forward"],
+ preparedData["right-raw-fast-backward"]);
+
+ // Create the distinct filtered datasets and store them
+ m_filteredDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
+ CombineDatasets(slowForward, slowBackward, fastForward, fastBackward);
+ m_filteredDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
+ CombineDatasets(preparedData["left-slow-forward"],
+ preparedData["left-slow-backward"],
+ preparedData["left-fast-forward"],
+ preparedData["left-fast-backward"]);
+ m_filteredDataset[static_cast<int>(
+ AnalysisManager::Settings::DrivetrainDataset::kRight)] =
+ CombineDatasets(preparedData["right-slow-forward"],
+ preparedData["right-slow-backward"],
+ preparedData["right-fast-forward"],
+ preparedData["right-fast-backward"]);
+
+ m_startTimes = {
+ rawSlowForward.front().timestamp, rawSlowBackward.front().timestamp,
+ rawFastForward.front().timestamp, rawFastBackward.front().timestamp};
+}
+
+AnalysisManager::AnalysisManager(Settings& settings, wpi::Logger& logger)
+ : m_logger{logger},
+ m_settings{settings},
+ m_type{analysis::kSimple},
+ m_unit{"Meters"},
+ m_factor{1} {}
+
+AnalysisManager::AnalysisManager(std::string_view path, Settings& settings,
+ wpi::Logger& logger)
+ : m_logger{logger}, m_settings{settings} {
+ {
+ // Read JSON from the specified path
+ std::error_code ec;
+ std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+ wpi::MemoryBuffer::GetFile(path, ec);
+ if (fileBuffer == nullptr || ec) {
+ throw FileReadingError(path);
+ }
+
+ m_json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+
+ WPI_INFO(m_logger, "Read {}", path);
+ }
+
+ // Check that we have a sysid JSON
+ if (m_json.find("sysid") == m_json.end()) {
+ // If it's not a sysid JSON, try converting it from frc-char format
+ std::string newPath = sysid::ConvertJSON(path, logger);
+
+ // Read JSON from the specified path
+ std::error_code ec;
+ std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+ wpi::MemoryBuffer::GetFile(path, ec);
+ if (fileBuffer == nullptr || ec) {
+ throw FileReadingError(newPath);
+ }
+
+ m_json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+
+ WPI_INFO(m_logger, "Read {}", newPath);
+ }
+
+ WPI_INFO(m_logger, "Parsing initial data of {}", path);
+ // Get the analysis type from the JSON.
+ m_type = sysid::analysis::FromName(m_json.at("test").get<std::string>());
+
+ // Get the rotation -> output units factor from the JSON.
+ m_unit = m_json.at("units").get<std::string>();
+ m_factor = m_json.at("unitsPerRotation").get<double>();
+ WPI_DEBUG(m_logger, "Parsing units per rotation as {} {} per rotation",
+ m_factor, m_unit);
+
+ // Reset settings for Dynamic Test Limits
+ m_settings.stepTestDuration = units::second_t{0.0};
+ m_settings.motionThreshold = std::numeric_limits<double>::infinity();
+}
+
+void AnalysisManager::PrepareData() {
+ WPI_INFO(m_logger, "Preparing {} data", m_type.name);
+ if (m_type == analysis::kDrivetrain) {
+ PrepareLinearDrivetrainData();
+ } else if (m_type == analysis::kDrivetrainAngular) {
+ PrepareAngularDrivetrainData();
+ } else {
+ PrepareGeneralData();
+ }
+ WPI_INFO(m_logger, "{}", "Finished Preparing Data");
+}
+
+AnalysisManager::FeedforwardGains AnalysisManager::CalculateFeedforward() {
+ if (m_filteredDataset.empty()) {
+ throw sysid::InvalidDataError(
+ "There is no data to perform gain calculation on.");
+ }
+
+ WPI_INFO(m_logger, "{}", "Calculating Gains");
+ // Calculate feedforward gains from the data.
+ const auto& ff = sysid::CalculateFeedforwardGains(GetFilteredData(), m_type);
+ FeedforwardGains ffGains = {ff, m_trackWidth};
+
+ const auto& Ks = std::get<0>(ff)[0];
+ const auto& Kv = std::get<0>(ff)[1];
+ const auto& Ka = std::get<0>(ff)[2];
+
+ if (Ka <= 0 || Kv < 0) {
+ throw InvalidDataError(
+ fmt::format("The calculated feedforward gains of kS: {}, Kv: {}, Ka: "
+ "{} are erroneous. Your Ka should be > 0 while your Kv and "
+ "Ks constants should both >= 0. Try adjusting the "
+ "filtering and trimming settings or collect better data.",
+ Ks, Kv, Ka));
+ }
+
+ return ffGains;
+}
+
+sysid::FeedbackGains AnalysisManager::CalculateFeedback(
+ std::vector<double> ff) {
+ const auto& Kv = ff[1];
+ const auto& Ka = ff[2];
+ FeedbackGains fb;
+ if (m_settings.type == FeedbackControllerLoopType::kPosition) {
+ fb = sysid::CalculatePositionFeedbackGains(
+ m_settings.preset, m_settings.lqr, Kv, Ka,
+ m_settings.convertGainsToEncTicks
+ ? m_settings.gearing * m_settings.cpr * m_factor
+ : 1);
+ } else {
+ fb = sysid::CalculateVelocityFeedbackGains(
+ m_settings.preset, m_settings.lqr, Kv, Ka,
+ m_settings.convertGainsToEncTicks
+ ? m_settings.gearing * m_settings.cpr * m_factor
+ : 1);
+ }
+
+ return fb;
+}
+
+void AnalysisManager::OverrideUnits(std::string_view unit,
+ double unitsPerRotation) {
+ m_unit = unit;
+ m_factor = unitsPerRotation;
+}
+
+void AnalysisManager::ResetUnitsFromJSON() {
+ m_unit = m_json.at("units").get<std::string>();
+ m_factor = m_json.at("unitsPerRotation").get<double>();
+}
diff --git a/sysid/src/main/native/cpp/analysis/AnalysisType.cpp b/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
new file mode 100644
index 0000000..18b461f
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/AnalysisType.h"
+
+using namespace sysid;
+
+AnalysisType sysid::analysis::FromName(std::string_view name) {
+ if (name == "Drivetrain") {
+ return sysid::analysis::kDrivetrain;
+ }
+ if (name == "Drivetrain (Angular)") {
+ return sysid::analysis::kDrivetrainAngular;
+ }
+ if (name == "Elevator") {
+ return sysid::analysis::kElevator;
+ }
+ if (name == "Arm") {
+ return sysid::analysis::kArm;
+ }
+ return sysid::analysis::kSimple;
+}
diff --git a/sysid/src/main/native/cpp/analysis/ArmSim.cpp b/sysid/src/main/native/cpp/analysis/ArmSim.cpp
new file mode 100644
index 0000000..a72f505
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/ArmSim.cpp
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/ArmSim.h"
+
+#include <cmath>
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/system/NumericalIntegration.h>
+#include <wpi/MathExtras.h>
+
+using namespace sysid;
+
+ArmSim::ArmSim(double Ks, double Kv, double Ka, double Kg, double offset,
+ double initialPosition, double initialVelocity)
+ // u = Ks sgn(x) + Kv x + Ka a + Kg cos(theta)
+ // Ka a = u - Ks sgn(x) - Kv x - Kg cos(theta)
+ // a = 1/Ka u - Ks/Ka sgn(x) - Kv/Ka x - Kg/Ka cos(theta)
+ // a = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(theta)
+ // a = Ax + Bu + c sgn(x) + d cos(theta)
+ : m_A{-Kv / Ka},
+ m_B{1.0 / Ka},
+ m_c{-Ks / Ka},
+ m_d{-Kg / Ka},
+ m_offset{offset} {
+ Reset(initialPosition, initialVelocity);
+}
+
+void ArmSim::Update(units::volt_t voltage, units::second_t dt) {
+ // Returns arm acceleration under gravity
+ auto f = [=, this](
+ const Eigen::Vector<double, 2>& x,
+ const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 2> {
+ return Eigen::Vector<double, 2>{
+ x(1), (m_A * x.block<1, 1>(1, 0) + m_B * u + m_c * wpi::sgn(x(1)) +
+ m_d * std::cos(x(0) + m_offset))(0)};
+ };
+
+ // Max error is large because an accurate sim isn't as important as the sim
+ // finishing in a timely manner. Otherwise, the timestep can become absurdly
+ // small for ill-conditioned data (e.g., high velocities with sharp spikes in
+ // acceleration).
+ Eigen::Vector<double, 1> u{voltage.value()};
+ m_x = frc::RKDP(f, m_x, u, dt, 0.25);
+}
+
+double ArmSim::GetPosition() const {
+ return m_x(0);
+}
+
+double ArmSim::GetVelocity() const {
+ return m_x(1);
+}
+
+double ArmSim::GetAcceleration(units::volt_t voltage) const {
+ Eigen::Vector<double, 1> u{voltage.value()};
+ return (m_A * m_x.block<1, 1>(1, 0) + m_B * u +
+ m_c * wpi::sgn(GetVelocity()) + m_d * std::cos(m_x(0) + m_offset))(0);
+}
+
+void ArmSim::Reset(double position, double velocity) {
+ m_x = Eigen::Vector<double, 2>{position, velocity};
+}
diff --git a/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp b/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp
new file mode 100644
index 0000000..5cfcabe
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp
@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/ElevatorSim.h"
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/system/Discretization.h>
+#include <wpi/MathExtras.h>
+
+using namespace sysid;
+
+ElevatorSim::ElevatorSim(double Ks, double Kv, double Ka, double Kg,
+ double initialPosition, double initialVelocity)
+ // dx/dt = Ax + Bu + c sgn(x) + d
+ : m_A{{0.0, 1.0}, {0.0, -Kv / Ka}},
+ m_B{0.0, 1.0 / Ka},
+ m_c{0.0, -Ks / Ka},
+ m_d{0.0, -Kg / Ka} {
+ Reset(initialPosition, initialVelocity);
+}
+
+void ElevatorSim::Update(units::volt_t voltage, units::second_t dt) {
+ Eigen::Vector<double, 1> u{voltage.value()};
+
+ // Given dx/dt = Ax + Bu + c sgn(x) + d,
+ // x_k+1 = e^(AT) x_k + A^-1 (e^(AT) - 1) (Bu + c sgn(x) + d)
+ Eigen::Matrix<double, 2, 2> Ad;
+ Eigen::Matrix<double, 2, 1> Bd;
+ frc::DiscretizeAB<2, 1>(m_A, m_B, dt, &Ad, &Bd);
+ m_x = Ad * m_x + Bd * u +
+ Bd * m_B.householderQr().solve(m_c * wpi::sgn(GetVelocity()) + m_d);
+}
+
+double ElevatorSim::GetPosition() const {
+ return m_x(0);
+}
+
+double ElevatorSim::GetVelocity() const {
+ return m_x(1);
+}
+
+double ElevatorSim::GetAcceleration(units::volt_t voltage) const {
+ Eigen::Vector<double, 1> u{voltage.value()};
+ return (m_A * m_x + m_B * u + m_c * wpi::sgn(GetVelocity()) + m_d)(1);
+}
+
+void ElevatorSim::Reset(double position, double velocity) {
+ m_x = Eigen::Vector<double, 2>{position, velocity};
+}
diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
new file mode 100644
index 0000000..0691aaf
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/FeedbackAnalysis.h"
+
+#include <frc/controller/LinearQuadraticRegulator.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/acceleration.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
+#include "sysid/analysis/FeedbackControllerPreset.h"
+
+using namespace sysid;
+
+using Kv_t = decltype(1_V / 1_mps);
+using Ka_t = decltype(1_V / 1_mps_sq);
+
+FeedbackGains sysid::CalculatePositionFeedbackGains(
+ const FeedbackControllerPreset& preset, const LQRParameters& params,
+ double Kv, double Ka, double encFactor) {
+ // If acceleration requires no effort, velocity becomes an input for position
+ // control. We choose an appropriate model in this case to avoid numerical
+ // instabilities in the LQR.
+ if (Ka > 1E-7) {
+ // Create a position system from our feedforward gains.
+ auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
+ Kv_t(Kv), Ka_t(Ka));
+ // Create an LQR with 2 states to control -- position and velocity.
+ frc::LinearQuadraticRegulator<2, 1> controller{
+ system, {params.qp, params.qv}, {params.r}, preset.period};
+ // Compensate for any latency from sensor measurements, filtering, etc.
+ controller.LatencyCompensate(system, preset.period, 0.0_s);
+
+ return {controller.K(0, 0) * preset.outputConversionFactor / encFactor,
+ controller.K(0, 1) * preset.outputConversionFactor /
+ (encFactor * (preset.normalized ? 1 : preset.period.value()))};
+ }
+
+ // This is our special model to avoid instabilities in the LQR.
+ auto system = frc::LinearSystem<1, 1, 1>(
+ Eigen::Matrix<double, 1, 1>{0.0}, Eigen::Matrix<double, 1, 1>{1.0},
+ Eigen::Matrix<double, 1, 1>{1.0}, Eigen::Matrix<double, 1, 1>{0.0});
+ // Create an LQR with one state -- position.
+ frc::LinearQuadraticRegulator<1, 1> controller{
+ system, {params.qp}, {params.r}, preset.period};
+ // Compensate for any latency from sensor measurements, filtering, etc.
+ controller.LatencyCompensate(system, preset.period, 0.0_s);
+
+ return {Kv * controller.K(0, 0) * preset.outputConversionFactor / encFactor,
+ 0.0};
+}
+
+FeedbackGains sysid::CalculateVelocityFeedbackGains(
+ const FeedbackControllerPreset& preset, const LQRParameters& params,
+ double Kv, double Ka, double encFactor) {
+ // If acceleration for velocity control requires no effort, the feedback
+ // control gains approach zero. We special-case it here because numerical
+ // instabilities arise in LQR otherwise.
+ if (Ka < 1E-7) {
+ return {0.0, 0.0};
+ }
+
+ // Create a velocity system from our feedforward gains.
+ auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
+ Kv_t(Kv), Ka_t(Ka));
+ // Create an LQR controller with 1 state -- velocity.
+ frc::LinearQuadraticRegulator<1, 1> controller{
+ system, {params.qv}, {params.r}, preset.period};
+ // Compensate for any latency from sensor measurements, filtering, etc.
+ controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
+
+ return {controller.K(0, 0) * preset.outputConversionFactor /
+ (preset.outputVelocityTimeFactor * encFactor),
+ 0.0};
+}
diff --git a/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
new file mode 100644
index 0000000..b7a9fce
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/FeedforwardAnalysis.h"
+
+#include <cmath>
+
+#include <units/math.h>
+#include <units/time.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/analysis/OLS.h"
+
+using namespace sysid;
+
+/**
+ * Populates OLS data for (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ).
+ *
+ * @param d List of characterization data.
+ * @param type Type of system being identified.
+ * @param X Vector representation of X in y = Xβ.
+ * @param y Vector representation of y in y = Xβ.
+ */
+static void PopulateOLSData(const std::vector<PreparedData>& d,
+ const AnalysisType& type,
+ Eigen::Block<Eigen::MatrixXd> X,
+ Eigen::VectorBlock<Eigen::VectorXd> y) {
+ for (size_t sample = 0; sample < d.size(); ++sample) {
+ const auto& pt = d[sample];
+
+ // Add the velocity term (for α)
+ X(sample, 0) = pt.velocity;
+
+ // Add the voltage term (for β)
+ X(sample, 1) = pt.voltage;
+
+ // Add the intercept term (for γ)
+ X(sample, 2) = std::copysign(1, pt.velocity);
+
+ // Add test-specific variables
+ if (type == analysis::kElevator) {
+ // Add the gravity term (for Kg)
+ X(sample, 3) = 1.0;
+ } else if (type == analysis::kArm) {
+ // Add the cosine and sine terms (for Kg)
+ X(sample, 3) = pt.cos;
+ X(sample, 4) = pt.sin;
+ }
+
+ // Add the dependent variable (acceleration)
+ y(sample) = pt.acceleration;
+ }
+}
+
+std::tuple<std::vector<double>, double, double>
+sysid::CalculateFeedforwardGains(const Storage& data,
+ const AnalysisType& type) {
+ // Iterate through the data and add it to our raw vector.
+ const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
+
+ const auto size = slowForward.size() + slowBackward.size() +
+ fastForward.size() + fastBackward.size();
+
+ // Create a raw vector of doubles with our data in it.
+ Eigen::MatrixXd X{size, type.independentVariables};
+ Eigen::VectorXd y{size};
+
+ int rowOffset = 0;
+ PopulateOLSData(slowForward, type,
+ X.block(rowOffset, 0, slowForward.size(), X.cols()),
+ y.segment(rowOffset, slowForward.size()));
+
+ rowOffset += slowForward.size();
+ PopulateOLSData(slowBackward, type,
+ X.block(rowOffset, 0, slowBackward.size(), X.cols()),
+ y.segment(rowOffset, slowBackward.size()));
+
+ rowOffset += slowBackward.size();
+ PopulateOLSData(fastForward, type,
+ X.block(rowOffset, 0, fastForward.size(), X.cols()),
+ y.segment(rowOffset, fastForward.size()));
+
+ rowOffset += fastForward.size();
+ PopulateOLSData(fastBackward, type,
+ X.block(rowOffset, 0, fastBackward.size(), X.cols()),
+ y.segment(rowOffset, fastBackward.size()));
+
+ // Perform OLS with accel = alpha*vel + beta*voltage + gamma*signum(vel)
+ // OLS performs best with the noisiest variable as the dependent var,
+ // so we regress accel in terms of the other variables.
+ auto ols = sysid::OLS(X, y);
+ double alpha = std::get<0>(ols)[0]; // -Kv/Ka
+ double beta = std::get<0>(ols)[1]; // 1/Ka
+ double gamma = std::get<0>(ols)[2]; // -Ks/Ka
+
+ // Initialize gains list with Ks, Kv, and Ka
+ std::vector<double> gains{-gamma / beta, -alpha / beta, 1 / beta};
+
+ if (type == analysis::kElevator) {
+ // Add Kg to gains list
+ double delta = std::get<0>(ols)[3]; // -Kg/Ka
+ gains.emplace_back(-delta / beta);
+ }
+
+ if (type == analysis::kArm) {
+ double delta = std::get<0>(ols)[3]; // -Kg/Ka cos(offset)
+ double epsilon = std::get<0>(ols)[4]; // Kg/Ka sin(offset)
+
+ // Add Kg to gains list
+ gains.emplace_back(std::hypot(delta, epsilon) / beta);
+
+ // Add offset to gains list
+ gains.emplace_back(std::atan2(epsilon, -delta));
+ }
+
+ // Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only)
+ return std::tuple{gains, std::get<1>(ols), std::get<2>(ols)};
+}
diff --git a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
new file mode 100644
index 0000000..6c66ef8
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
@@ -0,0 +1,417 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/FilteringUtils.h"
+
+#include <limits>
+#include <numbers>
+#include <numeric>
+#include <stdexcept>
+#include <vector>
+
+#include <fmt/format.h>
+#include <frc/filter/LinearFilter.h>
+#include <frc/filter/MedianFilter.h>
+#include <units/math.h>
+#include <wpi/StringExtras.h>
+
+using namespace sysid;
+
+/**
+ * Helper function that throws if it detects that the data vector is too small
+ * for an operation of a certain window size.
+ *
+ * @param data The data that is being used.
+ * @param window The window size for the operation.
+ * @param operation The operation we're checking the size for (for error
+ * throwing purposes).
+ */
+static void CheckSize(const std::vector<PreparedData>& data, size_t window,
+ std::string_view operation) {
+ if (data.size() < window) {
+ throw sysid::InvalidDataError(
+ fmt::format("Not enough data to run {} which has a window size of {}.",
+ operation, window));
+ }
+}
+
+/**
+ * Helper function that determines if a certain key is storing raw data.
+ *
+ * @param key The key of the dataset
+ *
+ * @return True, if the key corresponds to a raw dataset.
+ */
+static bool IsRaw(std::string_view key) {
+ return wpi::contains(key, "raw") && !wpi::contains(key, "original");
+}
+
+/**
+ * Helper function that determines if a certain key is storing filtered data.
+ *
+ * @param key The key of the dataset
+ *
+ * @return True, if the key corresponds to a filtered dataset.
+ */
+static bool IsFiltered(std::string_view key) {
+ return !wpi::contains(key, "raw") && !wpi::contains(key, "original");
+}
+
+/**
+ * Fills in the rest of the PreparedData Structs for a PreparedData Vector.
+ *
+ * @param data A reference to a vector of the raw data.
+ * @param unit The units that the data is in (rotations, radians, or degrees)
+ * for arm mechanisms.
+ */
+static void PrepareMechData(std::vector<PreparedData>* data,
+ std::string_view unit = "") {
+ constexpr size_t kWindow = 3;
+
+ CheckSize(*data, kWindow, "Acceleration Calculation");
+
+ // Calculates the cosine of the position data for single jointed arm analysis
+ for (size_t i = 0; i < data->size(); ++i) {
+ auto& pt = data->at(i);
+
+ double cos = 0.0;
+ double sin = 0.0;
+ if (unit == "Radians") {
+ cos = std::cos(pt.position);
+ sin = std::sin(pt.position);
+ } else if (unit == "Degrees") {
+ cos = std::cos(pt.position * std::numbers::pi / 180.0);
+ sin = std::sin(pt.position * std::numbers::pi / 180.0);
+ } else if (unit == "Rotations") {
+ cos = std::cos(pt.position * 2 * std::numbers::pi);
+ sin = std::sin(pt.position * 2 * std::numbers::pi);
+ }
+ pt.cos = cos;
+ pt.sin = sin;
+ }
+
+ auto derivative =
+ CentralFiniteDifference<1, kWindow>(GetMeanTimeDelta(*data));
+
+ // Load the derivative filter with the first value for accurate initial
+ // behavior
+ for (size_t i = 0; i < kWindow; ++i) {
+ derivative.Calculate(data->at(0).velocity);
+ }
+
+ for (size_t i = (kWindow - 1) / 2; i < data->size(); ++i) {
+ data->at(i - (kWindow - 1) / 2).acceleration =
+ derivative.Calculate(data->at(i).velocity);
+ }
+
+ // Fill in accelerations past end of derivative filter
+ for (size_t i = data->size() - (kWindow - 1) / 2; i < data->size(); ++i) {
+ data->at(i).acceleration = 0.0;
+ }
+}
+
+std::tuple<units::second_t, units::second_t, units::second_t>
+sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
+ AnalysisManager::Settings* settings,
+ units::second_t minStepTime,
+ units::second_t maxStepTime) {
+ auto voltageBegins =
+ std::find_if(data->begin(), data->end(),
+ [](auto& datum) { return std::abs(datum.voltage) > 0; });
+
+ units::second_t firstTimestamp = voltageBegins->timestamp;
+ double firstPosition = voltageBegins->position;
+
+ auto motionBegins = std::find_if(
+ data->begin(), data->end(), [settings, firstPosition](auto& datum) {
+ return std::abs(datum.position - firstPosition) >
+ (settings->motionThreshold * datum.dt.value());
+ });
+
+ units::second_t positionDelay;
+ if (motionBegins != data->end()) {
+ positionDelay = motionBegins->timestamp - firstTimestamp;
+ } else {
+ positionDelay = 0_s;
+ }
+
+ auto maxAccel = std::max_element(
+ data->begin(), data->end(), [](const auto& a, const auto& b) {
+ return std::abs(a.acceleration) < std::abs(b.acceleration);
+ });
+
+ units::second_t velocityDelay;
+ if (maxAccel != data->end()) {
+ velocityDelay = maxAccel->timestamp - firstTimestamp;
+
+ // Trim data before max acceleration
+ data->erase(data->begin(), maxAccel);
+ } else {
+ velocityDelay = 0_s;
+ }
+
+ minStepTime = std::min(data->at(0).timestamp - firstTimestamp, minStepTime);
+
+ // If step duration hasn't been set yet, calculate a default (find the entry
+ // before the acceleration first hits zero)
+ if (settings->stepTestDuration <= minStepTime) {
+ // Get noise floor
+ const double accelNoiseFloor = GetNoiseFloor(
+ *data, kNoiseMeanWindow, [](auto&& pt) { return pt.acceleration; });
+ // Find latest element with nonzero acceleration
+ auto endIt = std::find_if(
+ data->rbegin(), data->rend(), [&](const PreparedData& entry) {
+ return std::abs(entry.acceleration) > accelNoiseFloor;
+ });
+
+ if (endIt != data->rend()) {
+ // Calculate default duration
+ settings->stepTestDuration = std::min(
+ endIt->timestamp - data->front().timestamp + minStepTime + 1_s,
+ maxStepTime);
+ } else {
+ settings->stepTestDuration = maxStepTime;
+ }
+ }
+
+ // Find first entry greater than the step test duration
+ auto maxIt =
+ std::find_if(data->begin(), data->end(), [&](PreparedData entry) {
+ return entry.timestamp - data->front().timestamp + minStepTime >
+ settings->stepTestDuration;
+ });
+
+ // Trim data beyond desired step test duration
+ if (maxIt != data->end()) {
+ data->erase(maxIt, data->end());
+ }
+ return std::make_tuple(minStepTime, positionDelay, velocityDelay);
+}
+
+double sysid::GetNoiseFloor(
+ const std::vector<PreparedData>& data, int window,
+ std::function<double(const PreparedData&)> accessorFunction) {
+ double sum = 0.0;
+ size_t step = window / 2;
+ auto averageFilter = frc::LinearFilter<double>::MovingAverage(window);
+ for (size_t i = 0; i < data.size(); i++) {
+ double mean = averageFilter.Calculate(accessorFunction(data[i]));
+ if (i >= step) {
+ sum += std::pow(accessorFunction(data[i - step]) - mean, 2);
+ }
+ }
+ return std::sqrt(sum / (data.size() - step));
+}
+
+units::second_t sysid::GetMeanTimeDelta(const std::vector<PreparedData>& data) {
+ std::vector<units::second_t> dts;
+
+ for (const auto& pt : data) {
+ if (pt.dt > 0_s && pt.dt < 500_ms) {
+ dts.emplace_back(pt.dt);
+ }
+ }
+
+ return std::accumulate(dts.begin(), dts.end(), 0_s) / dts.size();
+}
+
+units::second_t sysid::GetMeanTimeDelta(const Storage& data) {
+ std::vector<units::second_t> dts;
+
+ for (const auto& pt : data.slowForward) {
+ if (pt.dt > 0_s && pt.dt < 500_ms) {
+ dts.emplace_back(pt.dt);
+ }
+ }
+
+ for (const auto& pt : data.slowBackward) {
+ if (pt.dt > 0_s && pt.dt < 500_ms) {
+ dts.emplace_back(pt.dt);
+ }
+ }
+
+ for (const auto& pt : data.fastForward) {
+ if (pt.dt > 0_s && pt.dt < 500_ms) {
+ dts.emplace_back(pt.dt);
+ }
+ }
+
+ for (const auto& pt : data.fastBackward) {
+ if (pt.dt > 0_s && pt.dt < 500_ms) {
+ dts.emplace_back(pt.dt);
+ }
+ }
+
+ return std::accumulate(dts.begin(), dts.end(), 0_s) / dts.size();
+}
+
+void sysid::ApplyMedianFilter(std::vector<PreparedData>* data, int window) {
+ CheckSize(*data, window, "Median Filter");
+
+ frc::MedianFilter<double> medianFilter(window);
+
+ // Load the median filter with the first value for accurate initial behavior
+ for (int i = 0; i < window; i++) {
+ medianFilter.Calculate(data->at(0).velocity);
+ }
+
+ for (size_t i = (window - 1) / 2; i < data->size(); i++) {
+ data->at(i - (window - 1) / 2).velocity =
+ medianFilter.Calculate(data->at(i).velocity);
+ }
+
+ // Run the median filter for the last half window of datapoints by loading the
+ // median filter with the last recorded velocity value
+ for (size_t i = data->size() - (window - 1) / 2; i < data->size(); i++) {
+ data->at(i).velocity =
+ medianFilter.Calculate(data->at(data->size() - 1).velocity);
+ }
+}
+
+/**
+ * Removes a substring from a string reference
+ *
+ * @param str The std::string_view that needs modification
+ * @param removeStr The substring that needs to be removed
+ *
+ * @return an std::string without the specified substring
+ */
+static std::string RemoveStr(std::string_view str, std::string_view removeStr) {
+ size_t idx = str.find(removeStr);
+ if (idx == std::string_view::npos) {
+ return std::string{str};
+ } else {
+ return fmt::format("{}{}", str.substr(0, idx),
+ str.substr(idx + removeStr.size()));
+ }
+}
+
+/**
+ * Figures out the max duration of the Dynamic tests
+ *
+ * @param data The raw data String Map
+ *
+ * @return The maximum duration of the Dynamic Tests
+ */
+static units::second_t GetMaxStepTime(
+ wpi::StringMap<std::vector<PreparedData>>& data) {
+ auto maxStepTime = 0_s;
+ for (auto& it : data) {
+ auto key = it.first();
+ auto& dataset = it.getValue();
+
+ if (IsRaw(key) && wpi::contains(key, "fast")) {
+ auto duration = dataset.back().timestamp - dataset.front().timestamp;
+ if (duration > maxStepTime) {
+ maxStepTime = duration;
+ }
+ }
+ }
+ return maxStepTime;
+}
+
+void sysid::InitialTrimAndFilter(
+ wpi::StringMap<std::vector<PreparedData>>* data,
+ AnalysisManager::Settings* settings,
+ std::vector<units::second_t>& positionDelays,
+ std::vector<units::second_t>& velocityDelays, units::second_t& minStepTime,
+ units::second_t& maxStepTime, std::string_view unit) {
+ auto& preparedData = *data;
+
+ // Find the maximum Step Test Duration of the dynamic tests
+ maxStepTime = GetMaxStepTime(preparedData);
+
+ // Calculate Velocity Threshold if it hasn't been set yet
+ if (settings->motionThreshold == std::numeric_limits<double>::infinity()) {
+ for (auto& it : preparedData) {
+ auto key = it.first();
+ auto& dataset = it.getValue();
+ if (wpi::contains(key, "slow")) {
+ settings->motionThreshold =
+ std::min(settings->motionThreshold,
+ GetNoiseFloor(dataset, kNoiseMeanWindow,
+ [](auto&& pt) { return pt.velocity; }));
+ }
+ }
+ }
+
+ for (auto& it : preparedData) {
+ auto key = it.first();
+ auto& dataset = it.getValue();
+
+ // Trim quasistatic test data to remove all points where voltage is zero or
+ // velocity < motion threshold.
+ if (wpi::contains(key, "slow")) {
+ dataset.erase(std::remove_if(dataset.begin(), dataset.end(),
+ [&](const auto& pt) {
+ return std::abs(pt.voltage) <= 0 ||
+ std::abs(pt.velocity) <
+ settings->motionThreshold;
+ }),
+ dataset.end());
+
+ // Confirm there's still data
+ if (dataset.empty()) {
+ throw sysid::NoQuasistaticDataError();
+ }
+ }
+
+ // Apply Median filter
+ if (IsFiltered(key) && settings->medianWindow > 1) {
+ ApplyMedianFilter(&dataset, settings->medianWindow);
+ }
+
+ // Recalculate Accel and Cosine
+ PrepareMechData(&dataset, unit);
+
+ // Trims filtered Dynamic Test Data
+ if (IsFiltered(key) && wpi::contains(key, "fast")) {
+ // Get the filtered dataset name
+ auto filteredKey = RemoveStr(key, "raw-");
+
+ // Trim Filtered Data
+ auto [tempMinStepTime, positionDelay, velocityDelay] =
+ TrimStepVoltageData(&preparedData[filteredKey], settings, minStepTime,
+ maxStepTime);
+
+ positionDelays.emplace_back(positionDelay);
+ velocityDelays.emplace_back(velocityDelay);
+
+ // Set the Raw Data to start at the same time as the Filtered Data
+ auto startTime = preparedData[filteredKey].front().timestamp;
+ auto rawStart =
+ std::find_if(preparedData[key].begin(), preparedData[key].end(),
+ [&](auto&& pt) { return pt.timestamp == startTime; });
+ preparedData[key].erase(preparedData[key].begin(), rawStart);
+
+ // Confirm there's still data
+ if (preparedData[key].empty()) {
+ throw sysid::NoDynamicDataError();
+ }
+ }
+ }
+}
+
+void sysid::AccelFilter(wpi::StringMap<std::vector<PreparedData>>* data) {
+ auto& preparedData = *data;
+
+ // Remove points with acceleration = 0
+ for (auto& it : preparedData) {
+ auto& dataset = it.getValue();
+
+ for (size_t i = 0; i < dataset.size(); i++) {
+ if (dataset.at(i).acceleration == 0.0) {
+ dataset.erase(dataset.begin() + i);
+ i--;
+ }
+ }
+ }
+
+ // Confirm there's still data
+ if (std::any_of(preparedData.begin(), preparedData.end(),
+ [](const auto& it) { return it.getValue().empty(); })) {
+ throw sysid::InvalidDataError(
+ "Acceleration filtering has removed all data.");
+ }
+}
diff --git a/sysid/src/main/native/cpp/analysis/JSONConverter.cpp b/sysid/src/main/native/cpp/analysis/JSONConverter.cpp
new file mode 100644
index 0000000..9bab8c6
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/JSONConverter.cpp
@@ -0,0 +1,164 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/JSONConverter.h"
+
+#include <stdexcept>
+#include <string>
+
+#include <fmt/core.h>
+#include <fmt/format.h>
+#include <wpi/Logger.h>
+#include <wpi/MemoryBuffer.h>
+#include <wpi/fmt/raw_ostream.h>
+#include <wpi/json.h>
+#include <wpi/raw_ostream.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
+
+// Sizes of the arrays for new sysid data.
+static constexpr size_t kDrivetrainSize = 9;
+static constexpr size_t kGeneralSize = 4;
+
+// Indices for the old data.
+static constexpr size_t kTimestampCol = 0;
+static constexpr size_t kLVoltsCol = 3;
+static constexpr size_t kRVoltsCol = 4;
+static constexpr size_t kLPosCol = 5;
+static constexpr size_t kRPosCol = 6;
+static constexpr size_t kLVelCol = 7;
+static constexpr size_t kRVelCol = 8;
+
+static wpi::json GetJSON(std::string_view path, wpi::Logger& logger) {
+ std::error_code ec;
+ std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+ wpi::MemoryBuffer::GetFile(path, ec);
+ if (fileBuffer == nullptr || ec) {
+ throw std::runtime_error(fmt::format("Unable to read: {}", path));
+ }
+
+ wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+ WPI_INFO(logger, "Read frc-characterization JSON from {}", path);
+ return json;
+}
+
+std::string sysid::ConvertJSON(std::string_view path, wpi::Logger& logger) {
+ wpi::json ojson = GetJSON(path, logger);
+
+ auto type = sysid::analysis::FromName(ojson.at("test").get<std::string>());
+ auto factor = ojson.at("unitsPerRotation").get<double>();
+ auto unit = ojson.at("units").get<std::string>();
+
+ wpi::json json;
+ for (auto&& key : AnalysisManager::kJsonDataKeys) {
+ if (type == analysis::kDrivetrain) {
+ // Get the old data; create a vector for the new data; reserve the
+ // appropriate size for the new data.
+ auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
+ std::vector<std::array<double, kDrivetrainSize>> data;
+ data.reserve(odata.size());
+
+ // Transfer the data.
+ for (auto&& pt : odata) {
+ data.push_back(std::array<double, kDrivetrainSize>{
+ pt[kTimestampCol], pt[kLVoltsCol], pt[kRVoltsCol], pt[kLPosCol],
+ pt[kRPosCol], pt[kLVelCol], pt[kRVelCol], 0.0, 0.0});
+ }
+ json[key] = data;
+ } else {
+ // Get the old data; create a vector for the new data; reserve the
+ // appropriate size for the new data.
+ auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
+ std::vector<std::array<double, kGeneralSize>> data;
+ data.reserve(odata.size());
+
+ // Transfer the data.
+ for (auto&& pt : odata) {
+ data.push_back(std::array<double, kGeneralSize>{
+ pt[kTimestampCol], pt[kLVoltsCol], pt[kLPosCol], pt[kLVelCol]});
+ }
+ json[key] = data;
+ }
+ }
+ json["units"] = unit;
+ json["unitsPerRotation"] = factor;
+ json["test"] = type.name;
+ json["sysid"] = true;
+
+ // Write the new file with "_new" appended to it.
+ path.remove_suffix(std::string_view{".json"}.size());
+ std::string loc = fmt::format("{}_new.json", path);
+
+ sysid::SaveFile(json.dump(2), std::filesystem::path{loc});
+
+ WPI_INFO(logger, "Wrote new JSON to: {}", loc);
+ return loc;
+}
+
+std::string sysid::ToCSV(std::string_view path, wpi::Logger& logger) {
+ wpi::json json = GetJSON(path, logger);
+
+ auto type = sysid::analysis::FromName(json.at("test").get<std::string>());
+ auto factor = json.at("unitsPerRotation").get<double>();
+ auto unit = json.at("units").get<std::string>();
+ std::string_view abbreviation = GetAbbreviation(unit);
+
+ std::error_code ec;
+ // Naming: {sysid-json-name}(Test, Units).csv
+ path.remove_suffix(std::string_view{".json"}.size());
+ std::string loc = fmt::format("{} ({}, {}).csv", path, type.name, unit);
+ wpi::raw_fd_ostream outputFile{loc, ec};
+
+ if (ec) {
+ throw std::runtime_error("Unable to write to: " + loc);
+ }
+
+ fmt::print(outputFile, "Timestamp (s),Test,");
+ if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
+ fmt::print(
+ outputFile,
+ "Left Volts (V),Right Volts (V),Left Position ({0}),Right "
+ "Position ({0}),Left Velocity ({0}/s),Right Velocity ({0}/s),Gyro "
+ "Position (deg),Gyro Rate (deg/s)\n",
+ abbreviation);
+ } else {
+ fmt::print(outputFile, "Volts (V),Position({0}),Velocity ({0}/s)\n",
+ abbreviation);
+ }
+ outputFile << "\n";
+
+ for (auto&& key : AnalysisManager::kJsonDataKeys) {
+ if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
+ auto tempData =
+ json.at(key).get<std::vector<std::array<double, kDrivetrainSize>>>();
+ for (auto&& pt : tempData) {
+ fmt::print(outputFile, "{},{},{},{},{},{},{},{},{},{}\n",
+ pt[0], // Timestamp
+ key, // Test
+ pt[1], pt[2], // Left and Right Voltages
+ pt[3] * factor, pt[4] * factor, // Left and Right Positions
+ pt[5] * factor, pt[6] * factor, // Left and Right Velocity
+ pt[7], pt[8] // Gyro Position and Velocity
+ );
+ }
+ } else {
+ auto tempData =
+ json.at(key).get<std::vector<std::array<double, kGeneralSize>>>();
+ for (auto&& pt : tempData) {
+ fmt::print(outputFile, "{},{},{},{},{}\n",
+ pt[0], // Timestamp,
+ key, // Test
+ pt[1], // Voltage
+ pt[2] * factor, // Position
+ pt[3] * factor // Velocity
+ );
+ }
+ }
+ }
+ outputFile.flush();
+ WPI_INFO(logger, "Wrote CSV to: {}", loc);
+ return loc;
+}
diff --git a/sysid/src/main/native/cpp/analysis/OLS.cpp b/sysid/src/main/native/cpp/analysis/OLS.cpp
new file mode 100644
index 0000000..d095a48
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/OLS.cpp
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/OLS.h"
+
+#include <tuple>
+#include <vector>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+
+using namespace sysid;
+
+std::tuple<std::vector<double>, double, double> sysid::OLS(
+ const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
+ assert(X.rows() == y.rows());
+
+ // The linear model can be written as follows:
+ // y = Xβ + u, where y is the dependent observed variable, X is the matrix
+ // of independent variables, β is a vector of coefficients, and u is a
+ // vector of residuals.
+
+ // We want to minimize u² = uᵀu = (y - Xβ)ᵀ(y - Xβ).
+ // β = (XᵀX)⁻¹Xᵀy
+
+ // Calculate β that minimizes uᵀu.
+ Eigen::MatrixXd beta = (X.transpose() * X).llt().solve(X.transpose() * y);
+
+ // We will now calculate R² or the coefficient of determination, which
+ // tells us how much of the total variation (variation in y) can be
+ // explained by the regression model.
+
+ // We will first calculate the sum of the squares of the error, or the
+ // variation in error (SSE).
+ double SSE = (y - X * beta).squaredNorm();
+
+ int n = X.cols();
+
+ // Now we will calculate the total variation in y, known as SSTO.
+ double SSTO = ((y.transpose() * y) - (1.0 / n) * (y.transpose() * y)).value();
+
+ double rSquared = (SSTO - SSE) / SSTO;
+ double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - 3.0));
+ double RMSE = std::sqrt(SSE / n);
+
+ return {{beta.data(), beta.data() + beta.rows()}, adjRSquared, RMSE};
+}
diff --git a/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp b/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp
new file mode 100644
index 0000000..58a8b30
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/SimpleMotorSim.h"
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/system/Discretization.h>
+#include <wpi/MathExtras.h>
+
+using namespace sysid;
+
+SimpleMotorSim::SimpleMotorSim(double Ks, double Kv, double Ka,
+ double initialPosition, double initialVelocity)
+ // dx/dt = Ax + Bu + c sgn(x)
+ : m_A{{0.0, 1.0}, {0.0, -Kv / Ka}}, m_B{0.0, 1.0 / Ka}, m_c{0.0, -Ks / Ka} {
+ Reset(initialPosition, initialVelocity);
+}
+
+void SimpleMotorSim::Update(units::volt_t voltage, units::second_t dt) {
+ Eigen::Vector<double, 1> u{voltage.value()};
+
+ // Given dx/dt = Ax + Bu + c sgn(x),
+ // x_k+1 = e^(AT) x_k + A^-1 (e^(AT) - 1) (Bu + c sgn(x))
+ Eigen::Matrix<double, 2, 2> Ad;
+ Eigen::Matrix<double, 2, 1> Bd;
+ frc::DiscretizeAB<2, 1>(m_A, m_B, dt, &Ad, &Bd);
+ m_x = Ad * m_x + Bd * u +
+ Bd * m_B.householderQr().solve(m_c * wpi::sgn(GetVelocity()));
+}
+
+double SimpleMotorSim::GetPosition() const {
+ return m_x(0);
+}
+
+double SimpleMotorSim::GetVelocity() const {
+ return m_x(1);
+}
+
+double SimpleMotorSim::GetAcceleration(units::volt_t voltage) const {
+ Eigen::Vector<double, 1> u{voltage.value()};
+ return (m_A * m_x + m_B * u + m_c * wpi::sgn(GetVelocity()))(1);
+}
+
+void SimpleMotorSim::Reset(double position, double velocity) {
+ m_x = Eigen::Vector<double, 2>{position, velocity};
+}
diff --git a/sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp b/sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp
new file mode 100644
index 0000000..eebfe8c
--- /dev/null
+++ b/sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp
@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/TrackWidthAnalysis.h"
+
+#include <cmath>
+
+double sysid::CalculateTrackWidth(double l, double r, units::radian_t accum) {
+ // The below comes from solving ω = (vr − vl) / 2r for 2r.
+ return (std::abs(r) + std::abs(l)) / std::abs(accum.value());
+}
diff --git a/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp b/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp
new file mode 100644
index 0000000..ac97cdb
--- /dev/null
+++ b/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp
@@ -0,0 +1,275 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/telemetry/TelemetryManager.h"
+
+#include <algorithm>
+#include <cctype> // for ::tolower
+#include <numbers>
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+#include <fmt/chrono.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/Logger.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/timestamp.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisType.h"
+
+using namespace sysid;
+
+TelemetryManager::TelemetryManager(const Settings& settings,
+ wpi::Logger& logger,
+ nt::NetworkTableInstance instance)
+ : m_settings(settings), m_logger(logger), m_inst(instance) {}
+
+void TelemetryManager::BeginTest(std::string_view name) {
+ // Create a new test params instance for this test.
+ m_params =
+ TestParameters{name.starts_with("fast"), name.ends_with("forward"),
+ m_settings.mechanism == analysis::kDrivetrainAngular,
+ State::WaitingForEnable};
+
+ // Add this test to the list of running tests and set the running flag.
+ m_tests.push_back(std::string{name});
+ m_isRunningTest = true;
+
+ // Set the Voltage Command Entry
+ m_voltageCommand.Set((m_params.fast ? m_settings.stepVoltage
+ : m_settings.quasistaticRampRate) *
+ (m_params.forward ? 1 : -1));
+
+ // Set the test type
+ m_testType.Set(m_params.fast ? "Dynamic" : "Quasistatic");
+
+ // Set the rotate entry
+ m_rotate.Set(m_params.rotate);
+
+ // Set the current mechanism in NT.
+ m_mechanism.Set(m_settings.mechanism.name);
+ // Set Overflow to False
+ m_overflowPub.Set(false);
+ // Set Mechanism Error to False
+ m_mechErrorPub.Set(false);
+ m_inst.Flush();
+
+ // Display the warning message.
+ for (auto&& func : m_callbacks) {
+ func(
+ "Please enable the robot in autonomous mode, and then "
+ "disable it "
+ "before it runs out of space. \n Note: The robot will "
+ "continue "
+ "to move until you disable it - It is your "
+ "responsibility to "
+ "ensure it does not hit anything!");
+ }
+
+ WPI_INFO(m_logger, "Started {} test.", m_tests.back());
+}
+
+void TelemetryManager::EndTest() {
+ // If there is no test running, this is a no-op
+ if (!m_isRunningTest) {
+ return;
+ }
+
+ // Disable the running flag and store the data in the JSON.
+ m_isRunningTest = false;
+ m_data[m_tests.back()] = m_params.data;
+
+ // Call the cancellation callbacks.
+ for (auto&& func : m_callbacks) {
+ std::string msg;
+ if (m_params.mechError) {
+ msg +=
+ "\nERROR: The robot indicated that you are using the wrong project "
+ "for characterizing your mechanism. \nThis most likely means you "
+ "are trying to characterize a mechanism like a Drivetrain with a "
+ "deployed config for a General Mechanism (e.g. Arm, Flywheel, and "
+ "Elevator) or vice versa. Please double check your settings and "
+ "try again.";
+ } else if (!m_params.data.empty()) {
+ std::string units = m_settings.units;
+ std::transform(m_settings.units.begin(), m_settings.units.end(),
+ units.begin(), ::tolower);
+
+ if (std::string_view{m_settings.mechanism.name}.starts_with(
+ "Drivetrain")) {
+ double p = (m_params.data.back()[3] - m_params.data.front()[3]) *
+ m_settings.unitsPerRotation;
+ double s = (m_params.data.back()[4] - m_params.data.front()[4]) *
+ m_settings.unitsPerRotation;
+ double g = m_params.data.back()[7] - m_params.data.front()[7];
+
+ msg = fmt::format(
+ "The left and right encoders traveled {} {} and {} {} "
+ "respectively.\nThe gyro angle delta was {} degrees.",
+ p, units, s, units, g * 180.0 / std::numbers::pi);
+ } else {
+ double p = (m_params.data.back()[2] - m_params.data.front()[2]) *
+ m_settings.unitsPerRotation;
+ msg = fmt::format("The encoder reported traveling {} {}.", p, units);
+ }
+
+ if (m_params.overflow) {
+ msg +=
+ "\nNOTE: the robot stopped recording data early because the entry "
+ "storage was exceeded.";
+ }
+ } else {
+ msg = "No data was detected.";
+ }
+ func(msg);
+ }
+
+ // Remove previously run test from list of tests if no data was detected.
+ if (m_params.data.empty()) {
+ m_tests.pop_back();
+ }
+
+ // Send a zero command over NT.
+ m_voltageCommand.Set(0.0);
+ m_inst.Flush();
+}
+
+void TelemetryManager::Update() {
+ // If there is no test running, these is nothing to update.
+ if (!m_isRunningTest) {
+ return;
+ }
+
+ // Update the NT entries that we're reading.
+
+ int currAckNumber = m_ackNumberSub.Get();
+ std::string telemetryValue;
+
+ // Get the FMS Control Word.
+ for (auto tsValue : m_fmsControlData.ReadQueue()) {
+ uint32_t ctrl = tsValue.value;
+ m_params.enabled = ctrl & 0x01;
+ }
+
+ // Get the string in the data field.
+ for (auto tsValue : m_telemetry.ReadQueue()) {
+ telemetryValue = tsValue.value;
+ }
+
+ // Get the overflow flag
+ for (auto tsValue : m_overflowSub.ReadQueue()) {
+ m_params.overflow = tsValue.value;
+ }
+
+ // Get the mechanism error flag
+ for (auto tsValue : m_mechErrorSub.ReadQueue()) {
+ m_params.mechError = tsValue.value;
+ }
+
+ // Go through our state machine.
+ if (m_params.state == State::WaitingForEnable) {
+ if (m_params.enabled) {
+ m_params.enableStart = wpi::Now() * 1E-6;
+ m_params.state = State::RunningTest;
+ m_ackNumber = currAckNumber;
+ WPI_INFO(m_logger, "{}", "Transitioned to running test state.");
+ }
+ }
+
+ if (m_params.state == State::RunningTest) {
+ // If for some reason we've disconnected, end the test.
+ if (!m_inst.IsConnected()) {
+ WPI_WARNING(m_logger, "{}",
+ "NT connection was dropped when executing the test. The test "
+ "has been canceled.");
+ EndTest();
+ }
+
+ // If the robot has disabled, then we can move on to the next step.
+ if (!m_params.enabled) {
+ m_params.disableStart = wpi::Now() * 1E-6;
+ m_params.state = State::WaitingForData;
+ WPI_INFO(m_logger, "{}", "Transitioned to waiting for data.");
+ }
+ }
+
+ if (m_params.state == State::WaitingForData) {
+ double now = wpi::Now() * 1E-6;
+ m_voltageCommand.Set(0.0);
+ m_inst.Flush();
+
+ // Process valid data
+ if (!telemetryValue.empty() && m_ackNumber < currAckNumber) {
+ m_params.raw = std::move(telemetryValue);
+ m_ackNumber = currAckNumber;
+ }
+
+ // We have the data that we need, so we can parse it and end the test.
+ if (!m_params.raw.empty() &&
+ wpi::starts_with(m_params.raw, m_tests.back())) {
+ // Remove test type from start of string
+ m_params.raw.erase(0, m_params.raw.find(';') + 1);
+
+ // Clean up the string -- remove spaces if there are any.
+ m_params.raw.erase(
+ std::remove_if(m_params.raw.begin(), m_params.raw.end(), ::isspace),
+ m_params.raw.end());
+
+ // Split the string into individual components.
+ wpi::SmallVector<std::string_view, 16> res;
+ wpi::split(m_params.raw, res, ',');
+
+ // Convert each string to double.
+ std::vector<double> values;
+ values.reserve(res.size());
+ for (auto&& str : res) {
+ values.push_back(wpi::parse_float<double>(str).value());
+ }
+
+ // Add the values to our result vector.
+ for (size_t i = 0; i < values.size() - m_settings.mechanism.rawDataSize;
+ i += m_settings.mechanism.rawDataSize) {
+ std::vector<double> d(m_settings.mechanism.rawDataSize);
+
+ std::copy_n(std::make_move_iterator(values.begin() + i),
+ m_settings.mechanism.rawDataSize, d.begin());
+ m_params.data.push_back(std::move(d));
+ }
+
+ WPI_INFO(m_logger,
+ "Received data with size: {} for the {} test in {} seconds.",
+ m_params.data.size(), m_tests.back(),
+ m_params.data.back()[0] - m_params.data.front()[0]);
+ m_ackNumberPub.Set(++m_ackNumber);
+ EndTest();
+ }
+
+ // If we timed out, end the test and let the user know.
+ if (now - m_params.disableStart > 5.0) {
+ WPI_WARNING(m_logger, "{}",
+ "TelemetryManager did not receieve data 5 seconds after "
+ "completing the test...");
+ EndTest();
+ }
+ }
+}
+
+std::string TelemetryManager::SaveJSON(std::string_view location) {
+ m_data["test"] = m_settings.mechanism.name;
+ m_data["units"] = m_settings.units;
+ m_data["unitsPerRotation"] = m_settings.unitsPerRotation;
+ m_data["sysid"] = true;
+
+ std::string loc = fmt::format("{}/sysid_data{:%Y%m%d-%H%M%S}.json", location,
+ std::chrono::system_clock::now());
+
+ sysid::SaveFile(m_data.dump(2), std::filesystem::path{loc});
+ WPI_INFO(m_logger, "Wrote JSON to: {}", loc);
+
+ return loc;
+}
diff --git a/sysid/src/main/native/cpp/view/Analyzer.cpp b/sysid/src/main/native/cpp/view/Analyzer.cpp
new file mode 100644
index 0000000..3270918
--- /dev/null
+++ b/sysid/src/main/native/cpp/view/Analyzer.cpp
@@ -0,0 +1,851 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/view/Analyzer.h"
+
+#include <algorithm>
+#include <exception>
+#include <filesystem>
+#include <numbers>
+#include <thread>
+
+#include <fmt/core.h>
+#include <glass/Context.h>
+#include <glass/Storage.h>
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <wpi/json.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/view/UILayout.h"
+
+using namespace sysid;
+
+Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger)
+ : m_location(""), m_logger(logger) {
+ // Fill the StringMap with preset values.
+ m_presets["Default"] = presets::kDefault;
+ m_presets["WPILib (2020-)"] = presets::kWPILibNew;
+ m_presets["WPILib (Pre-2020)"] = presets::kWPILibOld;
+ m_presets["CANCoder"] = presets::kCTRECANCoder;
+ m_presets["CTRE"] = presets::kCTREDefault;
+ m_presets["CTRE (Pro)"] = presets::kCTREProDefault;
+ m_presets["REV Brushless Encoder Port"] = presets::kREVNEOBuiltIn;
+ m_presets["REV Brushed Encoder Port"] = presets::kREVNonNEO;
+ m_presets["REV Data Port"] = presets::kREVNonNEO;
+ m_presets["Venom"] = presets::kVenom;
+
+ ResetData();
+ UpdateFeedbackGains();
+}
+
+void Analyzer::UpdateFeedforwardGains() {
+ WPI_INFO(m_logger, "{}", "Gain calc");
+ try {
+ const auto& [ff, trackWidth] = m_manager->CalculateFeedforward();
+ m_ff = std::get<0>(ff);
+ m_accelRSquared = std::get<1>(ff);
+ m_accelRMSE = std::get<2>(ff);
+ m_trackWidth = trackWidth;
+ m_settings.preset.measurementDelay =
+ m_settings.type == FeedbackControllerLoopType::kPosition
+ ? m_manager->GetPositionDelay()
+ : m_manager->GetVelocityDelay();
+ m_conversionFactor = m_manager->GetFactor();
+ PrepareGraphs();
+ } catch (const sysid::InvalidDataError& e) {
+ m_state = AnalyzerState::kGeneralDataError;
+ HandleError(e.what());
+ } catch (const sysid::NoQuasistaticDataError& e) {
+ m_state = AnalyzerState::kMotionThresholdError;
+ HandleError(e.what());
+ } catch (const sysid::NoDynamicDataError& e) {
+ m_state = AnalyzerState::kTestDurationError;
+ HandleError(e.what());
+ } catch (const AnalysisManager::FileReadingError& e) {
+ m_state = AnalyzerState::kFileError;
+ HandleError(e.what());
+ } catch (const wpi::json::exception& e) {
+ m_state = AnalyzerState::kFileError;
+ HandleError(e.what());
+ } catch (const std::exception& e) {
+ m_state = AnalyzerState::kFileError;
+ HandleError(e.what());
+ }
+}
+
+void Analyzer::UpdateFeedbackGains() {
+ if (m_ff[1] > 0 && m_ff[2] > 0) {
+ const auto& fb = m_manager->CalculateFeedback(m_ff);
+ m_timescale = units::second_t{m_ff[2] / m_ff[1]};
+ m_Kp = fb.Kp;
+ m_Kd = fb.Kd;
+ }
+}
+
+bool Analyzer::DisplayGain(const char* text, double* data,
+ bool readOnly = true) {
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
+ if (readOnly) {
+ return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G",
+ ImGuiInputTextFlags_ReadOnly);
+ } else {
+ return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G");
+ }
+}
+
+static void SetPosition(double beginX, double beginY, double xShift,
+ double yShift) {
+ ImGui::SetCursorPos(ImVec2(beginX + xShift * 10 * ImGui::GetFontSize(),
+ beginY + yShift * 1.75 * ImGui::GetFontSize()));
+}
+
+bool Analyzer::IsErrorState() {
+ return m_state == AnalyzerState::kMotionThresholdError ||
+ m_state == AnalyzerState::kTestDurationError ||
+ m_state == AnalyzerState::kGeneralDataError ||
+ m_state == AnalyzerState::kFileError;
+}
+
+bool Analyzer::IsDataErrorState() {
+ return m_state == AnalyzerState::kMotionThresholdError ||
+ m_state == AnalyzerState::kTestDurationError ||
+ m_state == AnalyzerState::kGeneralDataError;
+}
+
+void Analyzer::DisplayFileSelector() {
+ // Get the current width of the window. This will be used to scale
+ // our UI elements.
+ float width = ImGui::GetContentRegionAvail().x;
+
+ // Show the file location along with an option to choose.
+ if (ImGui::Button("Select")) {
+ m_selector = std::make_unique<pfd::open_file>(
+ "Select Data", "",
+ std::vector<std::string>{"JSON File", SYSID_PFD_JSON_EXT});
+ }
+ ImGui::SameLine();
+ ImGui::SetNextItemWidth(width - ImGui::CalcTextSize("Select").x -
+ ImGui::GetFontSize() * 5);
+ ImGui::InputText("##location", &m_location, ImGuiInputTextFlags_ReadOnly);
+}
+
+void Analyzer::ResetData() {
+ m_plot.ResetData();
+ m_manager = std::make_unique<AnalysisManager>(m_settings, m_logger);
+ m_location = "";
+ m_ff = std::vector<double>{1, 1, 1};
+ UpdateFeedbackGains();
+}
+
+bool Analyzer::DisplayResetAndUnitOverride() {
+ auto type = m_manager->GetAnalysisType();
+ auto unit = m_manager->GetUnit();
+
+ float width = ImGui::GetContentRegionAvail().x;
+ ImGui::SameLine(width - ImGui::CalcTextSize("Reset").x);
+ if (ImGui::Button("Reset")) {
+ ResetData();
+ m_state = AnalyzerState::kWaitingForJSON;
+ return true;
+ }
+
+ if (type == analysis::kDrivetrain) {
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+ if (ImGui::Combo("Dataset", &m_dataset, kDatasets, 3)) {
+ m_settings.dataset =
+ static_cast<AnalysisManager::Settings::DrivetrainDataset>(m_dataset);
+ PrepareData();
+ }
+ ImGui::SameLine();
+ } else {
+ m_settings.dataset =
+ AnalysisManager::Settings::DrivetrainDataset::kCombined;
+ }
+
+ ImGui::Spacing();
+ ImGui::Text(
+ "Units: %s\n"
+ "Units Per Rotation: %.4f\n"
+ "Type: %s",
+ std::string(unit).c_str(), m_conversionFactor, type.name);
+
+ if (type == analysis::kDrivetrainAngular) {
+ ImGui::SameLine();
+ sysid::CreateTooltip(
+ "Here, the units and units per rotation represent what the wheel "
+ "positions and velocities were captured in. The track width value "
+ "will reflect the unit selected here. However, the Kv and Ka will "
+ "always be in Vs/rad and Vs^2 / rad respectively.");
+ }
+
+ if (ImGui::Button("Override Units")) {
+ ImGui::OpenPopup("Override Units");
+ }
+
+ auto size = ImGui::GetIO().DisplaySize;
+ ImGui::SetNextWindowSize(ImVec2(size.x / 4, size.y * 0.2));
+ if (ImGui::BeginPopupModal("Override Units")) {
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
+ ImGui::Combo("Units", &m_selectedOverrideUnit, kUnits,
+ IM_ARRAYSIZE(kUnits));
+ unit = kUnits[m_selectedOverrideUnit];
+
+ if (unit == "Degrees") {
+ m_conversionFactor = 360.0;
+ } else if (unit == "Radians") {
+ m_conversionFactor = 2 * std::numbers::pi;
+ } else if (unit == "Rotations") {
+ m_conversionFactor = 1.0;
+ }
+
+ bool isRotational = m_selectedOverrideUnit > 2;
+
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
+ ImGui::InputDouble(
+ "Units Per Rotation", &m_conversionFactor, 0.0, 0.0, "%.4f",
+ isRotational ? ImGuiInputTextFlags_ReadOnly : ImGuiInputTextFlags_None);
+
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ m_manager->OverrideUnits(unit, m_conversionFactor);
+ PrepareData();
+ }
+
+ ImGui::EndPopup();
+ }
+
+ ImGui::SameLine();
+ if (ImGui::Button("Reset Units from JSON")) {
+ m_manager->ResetUnitsFromJSON();
+ PrepareData();
+ }
+
+ return false;
+}
+
+void Analyzer::ConfigParamsOnFileSelect() {
+ WPI_INFO(m_logger, "{}", "Configuring Params");
+ m_stepTestDuration = m_settings.stepTestDuration.to<float>();
+
+ // Estimate qp as 1/8 * units-per-rot
+ m_settings.lqr.qp = 0.125 * m_manager->GetFactor();
+ // Estimate qv as 1/4 * max velocity = 1/4 * (12V - kS) / kV
+ m_settings.lqr.qv = 0.25 * (12.0 - m_ff[0]) / m_ff[1];
+}
+
+void Analyzer::Display() {
+ DisplayFileSelector();
+ DisplayGraphs();
+
+ switch (m_state) {
+ case AnalyzerState::kWaitingForJSON: {
+ ImGui::Text(
+ "SysId is currently in theoretical analysis mode.\n"
+ "To analyze recorded test data, select a "
+ "data JSON.");
+ sysid::CreateTooltip(
+ "Theoretical feedback gains can be calculated from a "
+ "physical model of the mechanism being controlled. "
+ "Theoretical gains for several common mechanisms can "
+ "be obtained from ReCalc (https://reca.lc).");
+ ImGui::Spacing();
+ ImGui::Spacing();
+
+ ImGui::SetNextItemOpen(true, ImGuiCond_Once);
+ if (ImGui::CollapsingHeader("Feedforward Gains (Theoretical)")) {
+ float beginX = ImGui::GetCursorPosX();
+ float beginY = ImGui::GetCursorPosY();
+ CollectFeedforwardGains(beginX, beginY);
+ }
+ ImGui::SetNextItemOpen(true, ImGuiCond_Once);
+ if (ImGui::CollapsingHeader("Feedback Analysis")) {
+ DisplayFeedbackGains();
+ }
+ break;
+ }
+ case AnalyzerState::kNominalDisplay: { // Allow the user to select which
+ // data set they want analyzed and
+ // add a
+ // reset button. Also show the units and the units per rotation.
+ if (DisplayResetAndUnitOverride()) {
+ return;
+ }
+ ImGui::Spacing();
+ ImGui::Spacing();
+
+ ImGui::SetNextItemOpen(true, ImGuiCond_Once);
+ if (ImGui::CollapsingHeader("Feedforward Analysis")) {
+ float beginX = ImGui::GetCursorPosX();
+ float beginY = ImGui::GetCursorPosY();
+ DisplayFeedforwardGains(beginX, beginY);
+ }
+ ImGui::SetNextItemOpen(true, ImGuiCond_Once);
+ if (ImGui::CollapsingHeader("Feedback Analysis")) {
+ DisplayFeedbackGains();
+ }
+ break;
+ }
+ case AnalyzerState::kFileError: {
+ CreateErrorPopup(m_errorPopup, m_exception);
+ if (!m_errorPopup) {
+ m_state = AnalyzerState::kWaitingForJSON;
+ return;
+ }
+ break;
+ }
+ case AnalyzerState::kGeneralDataError:
+ case AnalyzerState::kTestDurationError:
+ case AnalyzerState::kMotionThresholdError: {
+ CreateErrorPopup(m_errorPopup, m_exception);
+ if (DisplayResetAndUnitOverride()) {
+ return;
+ }
+ float beginX = ImGui::GetCursorPosX();
+ float beginY = ImGui::GetCursorPosY();
+ DisplayFeedforwardParameters(beginX, beginY);
+ break;
+ }
+ }
+
+ // Periodic functions
+ try {
+ SelectFile();
+ } catch (const AnalysisManager::FileReadingError& e) {
+ m_state = AnalyzerState::kFileError;
+ HandleError(e.what());
+ } catch (const wpi::json::exception& e) {
+ m_state = AnalyzerState::kFileError;
+ HandleError(e.what());
+ }
+}
+
+void Analyzer::PrepareData() {
+ try {
+ m_manager->PrepareData();
+ UpdateFeedforwardGains();
+ UpdateFeedbackGains();
+ } catch (const sysid::InvalidDataError& e) {
+ m_state = AnalyzerState::kGeneralDataError;
+ HandleError(e.what());
+ } catch (const sysid::NoQuasistaticDataError& e) {
+ m_state = AnalyzerState::kMotionThresholdError;
+ HandleError(e.what());
+ } catch (const sysid::NoDynamicDataError& e) {
+ m_state = AnalyzerState::kTestDurationError;
+ HandleError(e.what());
+ } catch (const AnalysisManager::FileReadingError& e) {
+ m_state = AnalyzerState::kFileError;
+ HandleError(e.what());
+ } catch (const wpi::json::exception& e) {
+ m_state = AnalyzerState::kFileError;
+ HandleError(e.what());
+ } catch (const std::exception& e) {
+ m_state = AnalyzerState::kFileError;
+ HandleError(e.what());
+ }
+}
+
+void Analyzer::PrepareRawGraphs() {
+ if (m_manager->HasData()) {
+ AbortDataPrep();
+ m_dataThread = std::thread([&] {
+ m_plot.SetRawData(m_manager->GetOriginalData(), m_manager->GetUnit(),
+ m_abortDataPrep);
+ });
+ }
+}
+
+void Analyzer::PrepareGraphs() {
+ if (m_manager->HasData()) {
+ WPI_INFO(m_logger, "{}", "Graph state");
+ AbortDataPrep();
+ m_dataThread = std::thread([&] {
+ m_plot.SetData(m_manager->GetRawData(), m_manager->GetFilteredData(),
+ m_manager->GetUnit(), m_ff, m_manager->GetStartTimes(),
+ m_manager->GetAnalysisType(), m_abortDataPrep);
+ });
+ UpdateFeedbackGains();
+ m_state = AnalyzerState::kNominalDisplay;
+ }
+}
+
+void Analyzer::HandleError(std::string_view msg) {
+ m_exception = msg;
+ m_errorPopup = true;
+ if (m_state == AnalyzerState::kFileError) {
+ m_location = "";
+ }
+ PrepareRawGraphs();
+}
+
+void Analyzer::DisplayGraphs() {
+ ImGui::SetNextWindowPos(ImVec2{kDiagnosticPlotWindowPos},
+ ImGuiCond_FirstUseEver);
+ ImGui::SetNextWindowSize(ImVec2{kDiagnosticPlotWindowSize},
+ ImGuiCond_FirstUseEver);
+ ImGui::Begin("Diagnostic Plots");
+
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+ if (ImGui::SliderFloat("Point Size", &m_plot.m_pointSize, 1, 2, "%.2f")) {
+ if (!IsErrorState()) {
+ PrepareGraphs();
+ } else {
+ PrepareRawGraphs();
+ }
+ }
+
+ ImGui::SameLine();
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+ const char* items[] = {"Forward", "Backward"};
+ if (ImGui::Combo("Direction", &m_plot.m_direction, items, 2)) {
+ if (!IsErrorState()) {
+ PrepareGraphs();
+ } else {
+ PrepareRawGraphs();
+ }
+ }
+
+ // If the plots were already loaded, store the scroll position. Else go to
+ // the last recorded scroll position if they have just been initialized
+ bool plotsLoaded = m_plot.DisplayPlots();
+ if (plotsLoaded) {
+ if (m_prevPlotsLoaded) {
+ m_graphScroll = ImGui::GetScrollY();
+ } else {
+ ImGui::SetScrollY(m_graphScroll);
+ }
+
+ // If a JSON is selected
+ if (m_state == AnalyzerState::kNominalDisplay) {
+ DisplayGain("Acceleration R²", &m_accelRSquared);
+ CreateTooltip(
+ "The coefficient of determination of the OLS fit of acceleration "
+ "versus velocity and voltage. Acceleration is extremely noisy, "
+ "so this is generally quite small.");
+
+ ImGui::SameLine();
+ DisplayGain("Acceleration RMSE", &m_accelRMSE);
+ CreateTooltip(
+ "The standard deviation of the residuals from the predicted "
+ "acceleration."
+ "This can be interpreted loosely as the mean measured disturbance "
+ "from the \"ideal\" system equation.");
+
+ DisplayGain("Sim velocity R²", m_plot.GetSimRSquared());
+ CreateTooltip(
+ "The coefficient of determination the simulated velocity. "
+ "Velocity is much less-noisy than acceleration, so this "
+ "is pretty close to 1 for a decent fit.");
+
+ ImGui::SameLine();
+ DisplayGain("Sim velocity RMSE", m_plot.GetSimRMSE());
+ CreateTooltip(
+ "The standard deviation of the residuals from the simulated velocity "
+ "predictions - essentially the size of the mean error of the "
+ "simulated model "
+ "in the recorded velocity units.");
+ }
+ }
+ m_prevPlotsLoaded = plotsLoaded;
+
+ ImGui::End();
+}
+
+void Analyzer::SelectFile() {
+ // If the selector exists and is ready with a result, we can store it.
+ if (m_selector && m_selector->ready() && !m_selector->result().empty()) {
+ // Store the location of the file and reset the selector.
+ WPI_INFO(m_logger, "Opening File: {}", m_selector->result()[0]);
+ m_location = m_selector->result()[0];
+ m_selector.reset();
+ WPI_INFO(m_logger, "{}", "Opened File");
+ m_manager =
+ std::make_unique<AnalysisManager>(m_location, m_settings, m_logger);
+ PrepareData();
+ m_dataset = 0;
+ m_settings.dataset =
+ AnalysisManager::Settings::DrivetrainDataset::kCombined;
+ ConfigParamsOnFileSelect();
+ UpdateFeedbackGains();
+ }
+}
+
+void Analyzer::AbortDataPrep() {
+ if (m_dataThread.joinable()) {
+ m_abortDataPrep = true;
+ m_dataThread.join();
+ m_abortDataPrep = false;
+ }
+}
+
+void Analyzer::DisplayFeedforwardParameters(float beginX, float beginY) {
+ // Increase spacing to not run into trackwidth in the normal analyzer view
+ constexpr double kHorizontalOffset = 0.9;
+ SetPosition(beginX, beginY, kHorizontalOffset, 0);
+
+ bool displayAll =
+ !IsErrorState() || m_state == AnalyzerState::kGeneralDataError;
+
+ if (displayAll) {
+ // Wait for enter before refresh so double digit entries like "15" don't
+ // prematurely refresh with "1". That can cause display stuttering.
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ int window = m_settings.medianWindow;
+ if (ImGui::InputInt("Window Size", &window, 0, 0,
+ ImGuiInputTextFlags_EnterReturnsTrue)) {
+ m_settings.medianWindow = std::clamp(window, 1, 15);
+ PrepareData();
+ }
+
+ CreateTooltip(
+ "The number of samples in the velocity median "
+ "filter's sliding window.");
+ }
+
+ if (displayAll || m_state == AnalyzerState::kMotionThresholdError) {
+ // Wait for enter before refresh so decimal inputs like "0.2" don't
+ // prematurely refresh with a velocity threshold of "0".
+ SetPosition(beginX, beginY, kHorizontalOffset, 1);
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ double threshold = m_settings.motionThreshold;
+ if (ImGui::InputDouble("Velocity Threshold", &threshold, 0.0, 0.0, "%.3f",
+ ImGuiInputTextFlags_EnterReturnsTrue)) {
+ m_settings.motionThreshold = std::max(0.0, threshold);
+ PrepareData();
+ }
+ CreateTooltip("Velocity data below this threshold will be ignored.");
+ }
+
+ if (displayAll || m_state == AnalyzerState::kTestDurationError) {
+ SetPosition(beginX, beginY, kHorizontalOffset, 2);
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ if (ImGui::SliderFloat("Test Duration", &m_stepTestDuration,
+ m_manager->GetMinStepTime().value(),
+ m_manager->GetMaxStepTime().value(), "%.2f")) {
+ m_settings.stepTestDuration = units::second_t{m_stepTestDuration};
+ PrepareData();
+ }
+ }
+}
+
+void Analyzer::CollectFeedforwardGains(float beginX, float beginY) {
+ SetPosition(beginX, beginY, 0, 0);
+ if (DisplayGain("Kv", &m_ff[1], false)) {
+ UpdateFeedbackGains();
+ }
+
+ SetPosition(beginX, beginY, 0, 1);
+ if (DisplayGain("Ka", &m_ff[2], false)) {
+ UpdateFeedbackGains();
+ }
+
+ SetPosition(beginX, beginY, 0, 2);
+ // Show Timescale
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ DisplayGain("Response Timescale (ms)",
+ reinterpret_cast<double*>(&m_timescale));
+ CreateTooltip(
+ "The characteristic timescale of the system response in milliseconds. "
+ "Both the control loop period and total signal delay should be "
+ "at least 3-5 times shorter than this to optimally control the "
+ "system.");
+}
+
+void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) {
+ SetPosition(beginX, beginY, 0, 0);
+ DisplayGain("Ks", &m_ff[0]);
+
+ SetPosition(beginX, beginY, 0, 1);
+ DisplayGain("Kv", &m_ff[1]);
+
+ SetPosition(beginX, beginY, 0, 2);
+ DisplayGain("Ka", &m_ff[2]);
+
+ SetPosition(beginX, beginY, 0, 3);
+ // Show Timescale
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ DisplayGain("Response Timescale (ms)",
+ reinterpret_cast<double*>(&m_timescale));
+ CreateTooltip(
+ "The characteristic timescale of the system response in milliseconds. "
+ "Both the control loop period and total signal delay should be "
+ "at least 3-5 times shorter than this to optimally control the "
+ "system.");
+
+ SetPosition(beginX, beginY, 0, 4);
+ auto positionDelay = m_manager->GetPositionDelay();
+ DisplayGain("Position Measurement Delay (ms)",
+ reinterpret_cast<double*>(&positionDelay));
+ CreateTooltip(
+ "The average elapsed time between the first application of "
+ "voltage and the first detected change in mechanism position "
+ "in the step-voltage tests. This includes CAN delays, and "
+ "may overestimate the true delay for on-motor-controller "
+ "feedback loops by up to 20ms.");
+
+ SetPosition(beginX, beginY, 0, 5);
+ auto velocityDelay = m_manager->GetVelocityDelay();
+ DisplayGain("Velocity Measurement Delay (ms)",
+ reinterpret_cast<double*>(&velocityDelay));
+ CreateTooltip(
+ "The average elapsed time between the first application of "
+ "voltage and the maximum calculated mechanism acceleration "
+ "in the step-voltage tests. This includes CAN delays, and "
+ "may overestimate the true delay for on-motor-controller "
+ "feedback loops by up to 20ms.");
+
+ SetPosition(beginX, beginY, 0, 6);
+
+ if (m_manager->GetAnalysisType() == analysis::kElevator) {
+ DisplayGain("Kg", &m_ff[3]);
+ } else if (m_manager->GetAnalysisType() == analysis::kArm) {
+ DisplayGain("Kg", &m_ff[3]);
+
+ double offset;
+ auto unit = m_manager->GetUnit();
+ if (unit == "Radians") {
+ offset = m_ff[4];
+ } else if (unit == "Degrees") {
+ offset = m_ff[4] / std::numbers::pi * 180.0;
+ } else if (unit == "Rotations") {
+ offset = m_ff[4] / (2 * std::numbers::pi);
+ }
+ DisplayGain(
+ fmt::format("Angle offset to horizontal ({})", GetAbbreviation(unit))
+ .c_str(),
+ &offset);
+ CreateTooltip(
+ "This is the angle offset which, when added to the angle measurement, "
+ "zeroes it out when the arm is horizontal. This is needed for the arm "
+ "feedforward to work.");
+ } else if (m_trackWidth) {
+ DisplayGain("Track Width", &*m_trackWidth);
+ }
+ double endY = ImGui::GetCursorPosY();
+
+ DisplayFeedforwardParameters(beginX, beginY);
+ ImGui::SetCursorPosY(endY);
+}
+
+void Analyzer::DisplayFeedbackGains() {
+ // Allow the user to select a feedback controller preset.
+ ImGui::Spacing();
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+ if (ImGui::Combo("Gain Preset", &m_selectedPreset, kPresetNames,
+ IM_ARRAYSIZE(kPresetNames))) {
+ m_settings.preset = m_presets[kPresetNames[m_selectedPreset]];
+ m_settings.type = FeedbackControllerLoopType::kVelocity;
+ m_selectedLoopType =
+ static_cast<int>(FeedbackControllerLoopType::kVelocity);
+ m_settings.convertGainsToEncTicks = m_selectedPreset > 2;
+ UpdateFeedbackGains();
+ }
+ ImGui::SameLine();
+ sysid::CreateTooltip(
+ "Gain presets represent how feedback gains are calculated for your "
+ "specific feedback controller:\n\n"
+ "Default, WPILib (2020-): For use with the new WPILib PIDController "
+ "class.\n"
+ "WPILib (Pre-2020): For use with the old WPILib PIDController class.\n"
+ "CTRE: For use with CTRE units. These are the default units that ship "
+ "with CTRE motor controllers.\n"
+ "REV (Brushless): For use with NEO and NEO 550 motors on a SPARK MAX.\n"
+ "REV (Brushed): For use with brushed motors connected to a SPARK MAX.");
+
+ if (m_settings.preset != m_presets[kPresetNames[m_selectedPreset]]) {
+ ImGui::SameLine();
+ ImGui::TextDisabled("(modified)");
+ }
+
+ // Show our feedback controller preset values.
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ double value = m_settings.preset.outputConversionFactor * 12;
+ if (ImGui::InputDouble("Max Controller Output", &value, 0.0, 0.0, "%.1f") &&
+ value > 0) {
+ m_settings.preset.outputConversionFactor = value / 12.0;
+ UpdateFeedbackGains();
+ }
+
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ value = m_settings.preset.outputVelocityTimeFactor;
+ if (ImGui::InputDouble("Velocity Denominator Units (s)", &value, 0.0, 0.0,
+ "%.1f") &&
+ value > 0) {
+ m_settings.preset.outputVelocityTimeFactor = value;
+ UpdateFeedbackGains();
+ }
+
+ sysid::CreateTooltip(
+ "This represents the denominator of the velocity unit used by the "
+ "feedback controller. For example, CTRE uses 100 ms = 0.1 s.");
+
+ auto ShowPresetValue = [](const char* text, double* data,
+ float cursorX = 0.0f) {
+ if (cursorX > 0) {
+ ImGui::SetCursorPosX(cursorX);
+ }
+
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G");
+ };
+
+ // Show controller period.
+ if (ShowPresetValue("Controller Period (ms)",
+ reinterpret_cast<double*>(&m_settings.preset.period))) {
+ if (m_settings.preset.period > 0_s &&
+ m_settings.preset.measurementDelay >= 0_s) {
+ UpdateFeedbackGains();
+ }
+ }
+
+ // Show whether the controller gains are time-normalized.
+ if (ImGui::Checkbox("Time-Normalized?", &m_settings.preset.normalized)) {
+ UpdateFeedbackGains();
+ }
+
+ // Show position/velocity measurement delay.
+ if (ShowPresetValue(
+ "Measurement Delay (ms)",
+ reinterpret_cast<double*>(&m_settings.preset.measurementDelay))) {
+ if (m_settings.preset.period > 0_s &&
+ m_settings.preset.measurementDelay >= 0_s) {
+ UpdateFeedbackGains();
+ }
+ }
+
+ sysid::CreateTooltip(
+ "The average measurement delay of the process variable in milliseconds. "
+ "This may depend on your encoder settings and choice of motor "
+ "controller. Default velocity filtering windows are quite long "
+ "on many motor controllers, so be careful that this value is "
+ "accurate if the characteristic timescale of the mechanism "
+ "is small.");
+
+ // Add CPR and Gearing for converting Feedback Gains
+ ImGui::Separator();
+ ImGui::Spacing();
+
+ if (ImGui::Checkbox("Convert Gains to Encoder Counts",
+ &m_settings.convertGainsToEncTicks)) {
+ UpdateFeedbackGains();
+ }
+ sysid::CreateTooltip(
+ "Whether the feedback gains should be in terms of encoder counts or "
+ "output units. Because smart motor controllers usually don't have "
+ "direct access to the output units (i.e. m/s for a drivetrain), they "
+ "perform feedback on the encoder counts directly. If you are using a "
+ "PID Controller on the RoboRIO, you are probably performing feedback "
+ "on the output units directly.\n\nNote that if you have properly set "
+ "up position and velocity conversion factors with the SPARK MAX, you "
+ "can leave this box unchecked. The motor controller will perform "
+ "feedback on the output directly.");
+
+ if (m_settings.convertGainsToEncTicks) {
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
+ if (ImGui::InputDouble("##Numerator", &m_gearingNumerator, 0.0, 0.0, "%.4f",
+ ImGuiInputTextFlags_EnterReturnsTrue) &&
+ m_gearingNumerator > 0) {
+ m_settings.gearing = m_gearingNumerator / m_gearingDenominator;
+ UpdateFeedbackGains();
+ }
+ ImGui::SameLine();
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
+ if (ImGui::InputDouble("##Denominator", &m_gearingDenominator, 0.0, 0.0,
+ "%.4f", ImGuiInputTextFlags_EnterReturnsTrue) &&
+ m_gearingDenominator > 0) {
+ m_settings.gearing = m_gearingNumerator / m_gearingDenominator;
+ UpdateFeedbackGains();
+ }
+ sysid::CreateTooltip(
+ "The gearing between the encoder and the motor shaft (# of encoder "
+ "turns / # of motor shaft turns).");
+
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
+ if (ImGui::InputInt("CPR", &m_settings.cpr, 0, 0,
+ ImGuiInputTextFlags_EnterReturnsTrue) &&
+ m_settings.cpr > 0) {
+ UpdateFeedbackGains();
+ }
+ sysid::CreateTooltip(
+ "The counts per rotation of your encoder. This is the number of counts "
+ "reported in user code when the encoder is rotated exactly once. Some "
+ "common values for various motors/encoders are:\n\n"
+ "Falcon 500: 2048\nNEO: 1\nCTRE Mag Encoder / CANCoder: 4096\nREV "
+ "Through Bore Encoder: 8192\n");
+ }
+
+ ImGui::Separator();
+ ImGui::Spacing();
+
+ // Allow the user to select a loop type.
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+ if (ImGui::Combo("Loop Type", &m_selectedLoopType, kLoopTypes,
+ IM_ARRAYSIZE(kLoopTypes))) {
+ m_settings.type =
+ static_cast<FeedbackControllerLoopType>(m_selectedLoopType);
+ if (m_state == AnalyzerState::kWaitingForJSON) {
+ m_settings.preset.measurementDelay = 0_ms;
+ } else {
+ if (m_settings.type == FeedbackControllerLoopType::kPosition) {
+ m_settings.preset.measurementDelay = m_manager->GetPositionDelay();
+ } else {
+ m_settings.preset.measurementDelay = m_manager->GetVelocityDelay();
+ }
+ }
+ UpdateFeedbackGains();
+ }
+
+ ImGui::Spacing();
+
+ // Show Kp and Kd.
+ float beginY = ImGui::GetCursorPosY();
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ DisplayGain("Kp", &m_Kp);
+
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+ DisplayGain("Kd", &m_Kd);
+
+ // Come back to the starting y pos.
+ ImGui::SetCursorPosY(beginY);
+
+ if (m_selectedLoopType == 0) {
+ std::string unit;
+ if (m_state != AnalyzerState::kWaitingForJSON) {
+ unit = fmt::format(" ({})", GetAbbreviation(m_manager->GetUnit()));
+ }
+
+ ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
+ if (DisplayGain(fmt::format("Max Position Error{}", unit).c_str(),
+ &m_settings.lqr.qp, false)) {
+ if (m_settings.lqr.qp > 0) {
+ UpdateFeedbackGains();
+ }
+ }
+ }
+
+ std::string unit;
+ if (m_state != AnalyzerState::kWaitingForJSON) {
+ unit = fmt::format(" ({}/s)", GetAbbreviation(m_manager->GetUnit()));
+ }
+
+ ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
+ if (DisplayGain(fmt::format("Max Velocity Error{}", unit).c_str(),
+ &m_settings.lqr.qv, false)) {
+ if (m_settings.lqr.qv > 0) {
+ UpdateFeedbackGains();
+ }
+ }
+ ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
+ if (DisplayGain("Max Control Effort (V)", &m_settings.lqr.r, false)) {
+ if (m_settings.lqr.r > 0) {
+ UpdateFeedbackGains();
+ }
+ }
+}
diff --git a/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp b/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp
new file mode 100644
index 0000000..d8afbaf
--- /dev/null
+++ b/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp
@@ -0,0 +1,531 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/view/AnalyzerPlot.h"
+
+#include <algorithm>
+#include <cmath>
+#include <mutex>
+
+#include <fmt/format.h>
+#include <units/math.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/ArmSim.h"
+#include "sysid/analysis/ElevatorSim.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/analysis/SimpleMotorSim.h"
+
+using namespace sysid;
+
+static ImPlotPoint Getter(int idx, void* data) {
+ return static_cast<ImPlotPoint*>(data)[idx];
+}
+
+template <typename Model>
+static std::vector<std::vector<ImPlotPoint>> PopulateTimeDomainSim(
+ const std::vector<PreparedData>& data,
+ const std::array<units::second_t, 4>& startTimes, size_t step, Model model,
+ double* simSquaredErrorSum, double* squaredVariationSum,
+ int* timeSeriesPoints) {
+ // Create the vector of ImPlotPoints that will contain our simulated data.
+ std::vector<std::vector<ImPlotPoint>> pts;
+ std::vector<ImPlotPoint> tmp;
+
+ auto startTime = data[0].timestamp;
+
+ tmp.emplace_back(startTime.value(), data[0].velocity);
+
+ model.Reset(data[0].position, data[0].velocity);
+ units::second_t t = 0_s;
+
+ for (size_t i = 1; i < data.size(); ++i) {
+ const auto& now = data[i];
+ const auto& pre = data[i - 1];
+
+ t += now.timestamp - pre.timestamp;
+
+ // If the current time stamp and previous time stamp are across a test's
+ // start timestamp, it is the start of a new test and the model needs to be
+ // reset.
+ if (std::find(startTimes.begin(), startTimes.end(), now.timestamp) !=
+ startTimes.end()) {
+ pts.emplace_back(std::move(tmp));
+ model.Reset(now.position, now.velocity);
+ continue;
+ }
+
+ model.Update(units::volt_t{pre.voltage}, now.timestamp - pre.timestamp);
+ tmp.emplace_back((startTime + t).value(), model.GetVelocity());
+ *simSquaredErrorSum += std::pow(now.velocity - model.GetVelocity(), 2);
+ *squaredVariationSum += std::pow(now.velocity, 2);
+ ++(*timeSeriesPoints);
+ }
+
+ pts.emplace_back(std::move(tmp));
+ return pts;
+}
+
+AnalyzerPlot::AnalyzerPlot(wpi::Logger& logger) : m_logger(logger) {}
+
+void AnalyzerPlot::SetRawTimeData(const std::vector<PreparedData>& rawSlow,
+ const std::vector<PreparedData>& rawFast,
+ std::atomic<bool>& abort) {
+ auto rawSlowStep = std::ceil(rawSlow.size() * 1.0 / kMaxSize * 4);
+ auto rawFastStep = std::ceil(rawFast.size() * 1.0 / kMaxSize * 4);
+ // Populate Raw Slow Time Series Data
+ for (size_t i = 0; i < rawSlow.size(); i += rawSlowStep) {
+ if (abort) {
+ return;
+ }
+ m_quasistaticData.rawData.emplace_back((rawSlow[i].timestamp).value(),
+ rawSlow[i].velocity);
+ }
+
+ // Populate Raw fast Time Series Data
+ for (size_t i = 0; i < rawFast.size(); i += rawFastStep) {
+ if (abort) {
+ return;
+ }
+ m_dynamicData.rawData.emplace_back((rawFast[i].timestamp).value(),
+ rawFast[i].velocity);
+ }
+}
+
+void AnalyzerPlot::ResetData() {
+ m_quasistaticData.Clear();
+ m_dynamicData.Clear();
+ m_regressionData.Clear();
+ m_timestepData.Clear();
+
+ FitPlots();
+}
+
+void AnalyzerPlot::SetGraphLabels(std::string_view unit) {
+ std::string_view abbreviation = GetAbbreviation(unit);
+ m_velocityLabel = fmt::format("Velocity ({}/s)", abbreviation);
+ m_accelerationLabel = fmt::format("Acceleration ({}/s²)", abbreviation);
+ m_velPortionAccelLabel =
+ fmt::format("Velocity-Portion Accel ({}/s²)", abbreviation);
+}
+
+void AnalyzerPlot::SetRawData(const Storage& data, std::string_view unit,
+ std::atomic<bool>& abort) {
+ const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
+ const auto& slow = m_direction == 0 ? slowForward : slowBackward;
+ const auto& fast = m_direction == 0 ? fastForward : fastBackward;
+
+ SetGraphLabels(unit);
+
+ std::scoped_lock lock(m_mutex);
+
+ ResetData();
+ SetRawTimeData(slow, fast, abort);
+}
+
+void AnalyzerPlot::SetData(const Storage& rawData, const Storage& filteredData,
+ std::string_view unit,
+ const std::vector<double>& ffGains,
+ const std::array<units::second_t, 4>& startTimes,
+ AnalysisType type, std::atomic<bool>& abort) {
+ double simSquaredErrorSum = 0;
+ double squaredVariationSum = 0;
+ int timeSeriesPoints = 0;
+
+ const auto& Ks = ffGains[0];
+ const auto& Kv = ffGains[1];
+ const auto& Ka = ffGains[2];
+
+ auto& [slowForward, slowBackward, fastForward, fastBackward] = filteredData;
+ auto& [rawSlowForward, rawSlowBackward, rawFastForward, rawFastBackward] =
+ rawData;
+
+ const auto slow = AnalysisManager::DataConcat(slowForward, slowBackward);
+ const auto fast = AnalysisManager::DataConcat(fastForward, fastBackward);
+ const auto rawSlow =
+ AnalysisManager::DataConcat(rawSlowForward, rawSlowBackward);
+ const auto rawFast =
+ AnalysisManager::DataConcat(rawFastForward, rawFastBackward);
+
+ SetGraphLabels(unit);
+
+ std::scoped_lock lock(m_mutex);
+
+ ResetData();
+
+ // Calculate step sizes to ensure that we only use the memory that we
+ // allocated.
+ auto slowStep = std::ceil(slow.size() * 1.0 / kMaxSize * 4);
+ auto fastStep = std::ceil(fast.size() * 1.0 / kMaxSize * 4);
+
+ units::second_t dtMean = GetMeanTimeDelta(filteredData);
+
+ // Velocity-vs-time plots
+ {
+ const auto& slow = m_direction == 0 ? slowForward : slowBackward;
+ const auto& fast = m_direction == 0 ? fastForward : fastBackward;
+ const auto& rawSlow = m_direction == 0 ? rawSlowForward : rawSlowBackward;
+ const auto& rawFast = m_direction == 0 ? rawFastForward : rawFastBackward;
+
+ // Populate quasistatic time-domain graphs
+ for (size_t i = 0; i < slow.size(); i += slowStep) {
+ if (abort) {
+ return;
+ }
+
+ m_quasistaticData.filteredData.emplace_back((slow[i].timestamp).value(),
+ slow[i].velocity);
+
+ if (i > 0) {
+ // If the current timestamp is not in the startTimes array, it is the
+ // during a test and should be included. If it is in the startTimes
+ // array, it is the beginning of a new test and the dt will be inflated.
+ // Therefore we skip those to exclude that dt and effectively reset dt
+ // calculations.
+ if (slow[i].dt > 0_s &&
+ std::find(startTimes.begin(), startTimes.end(),
+ slow[i].timestamp) == startTimes.end()) {
+ m_timestepData.data.emplace_back(
+ (slow[i].timestamp).value(),
+ units::millisecond_t{slow[i].dt}.value());
+ }
+ }
+ }
+
+ // Populate dynamic time-domain graphs
+ for (size_t i = 0; i < fast.size(); i += fastStep) {
+ if (abort) {
+ return;
+ }
+
+ m_dynamicData.filteredData.emplace_back((fast[i].timestamp).value(),
+ fast[i].velocity);
+
+ if (i > 0) {
+ // If the current timestamp is not in the startTimes array, it is the
+ // during a test and should be included. If it is in the startTimes
+ // array, it is the beginning of a new test and the dt will be inflated.
+ // Therefore we skip those to exclude that dt and effectively reset dt
+ // calculations.
+ if (fast[i].dt > 0_s &&
+ std::find(startTimes.begin(), startTimes.end(),
+ fast[i].timestamp) == startTimes.end()) {
+ m_timestepData.data.emplace_back(
+ (fast[i].timestamp).value(),
+ units::millisecond_t{fast[i].dt}.value());
+ }
+ }
+ }
+
+ SetRawTimeData(rawSlow, rawFast, abort);
+
+ // Populate simulated time domain data
+ if (type == analysis::kElevator) {
+ const auto& Kg = ffGains[3];
+ m_quasistaticData.simData = PopulateTimeDomainSim(
+ rawSlow, startTimes, fastStep, sysid::ElevatorSim{Ks, Kv, Ka, Kg},
+ &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+ m_dynamicData.simData = PopulateTimeDomainSim(
+ rawFast, startTimes, fastStep, sysid::ElevatorSim{Ks, Kv, Ka, Kg},
+ &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+ } else if (type == analysis::kArm) {
+ const auto& Kg = ffGains[3];
+ const auto& offset = ffGains[4];
+ m_quasistaticData.simData = PopulateTimeDomainSim(
+ rawSlow, startTimes, fastStep, sysid::ArmSim{Ks, Kv, Ka, Kg, offset},
+ &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+ m_dynamicData.simData = PopulateTimeDomainSim(
+ rawFast, startTimes, fastStep, sysid::ArmSim{Ks, Kv, Ka, Kg, offset},
+ &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+ } else {
+ m_quasistaticData.simData = PopulateTimeDomainSim(
+ rawSlow, startTimes, fastStep, sysid::SimpleMotorSim{Ks, Kv, Ka},
+ &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+ m_dynamicData.simData = PopulateTimeDomainSim(
+ rawFast, startTimes, fastStep, sysid::SimpleMotorSim{Ks, Kv, Ka},
+ &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
+ }
+ }
+
+ // Acceleration-vs-velocity plot
+
+ // Find minimum velocity of slow and fast datasets, then find point for line
+ // of best fit
+ auto slowMinVel =
+ std::min_element(slow.cbegin(), slow.cend(), [](auto& a, auto& b) {
+ return a.velocity < b.velocity;
+ })->velocity;
+ auto fastMinVel =
+ std::min_element(fast.cbegin(), fast.cend(), [](auto& a, auto& b) {
+ return a.velocity < b.velocity;
+ })->velocity;
+ auto minVel = std::min(slowMinVel, fastMinVel);
+ m_regressionData.fitLine[0] = ImPlotPoint{minVel, -Kv / Ka * minVel};
+
+ // Find maximum velocity of slow and fast datasets, then find point for line
+ // of best fit
+ auto slowMaxVel =
+ std::max_element(slow.cbegin(), slow.cend(), [](auto& a, auto& b) {
+ return a.velocity < b.velocity;
+ })->velocity;
+ auto fastMaxVel =
+ std::max_element(fast.cbegin(), fast.cend(), [](auto& a, auto& b) {
+ return a.velocity < b.velocity;
+ })->velocity;
+ auto maxVel = std::max(slowMaxVel, fastMaxVel);
+ m_regressionData.fitLine[1] = ImPlotPoint{maxVel, -Kv / Ka * maxVel};
+
+ // Populate acceleration vs velocity graph
+ for (size_t i = 0; i < slow.size(); i += slowStep) {
+ if (abort) {
+ return;
+ }
+
+ // Calculate portion of acceleration caused by back-EMF
+ double accelPortion = slow[i].acceleration - 1.0 / Ka * slow[i].voltage +
+ std::copysign(Ks / Ka, slow[i].velocity);
+
+ if (type == analysis::kElevator) {
+ const auto& Kg = ffGains[3];
+ accelPortion -= Kg / Ka;
+ } else if (type == analysis::kArm) {
+ const auto& Kg = ffGains[3];
+ accelPortion -= Kg / Ka * slow[i].cos;
+ }
+
+ m_regressionData.data.emplace_back(slow[i].velocity, accelPortion);
+ }
+ for (size_t i = 0; i < fast.size(); i += fastStep) {
+ if (abort) {
+ return;
+ }
+
+ // Calculate portion of voltage that corresponds to change in acceleration.
+ double accelPortion = fast[i].acceleration - 1.0 / Ka * fast[i].voltage +
+ std::copysign(Ks / Ka, fast[i].velocity);
+
+ if (type == analysis::kElevator) {
+ const auto& Kg = ffGains[3];
+ accelPortion -= Kg / Ka;
+ } else if (type == analysis::kArm) {
+ const auto& Kg = ffGains[3];
+ accelPortion -= Kg / Ka * fast[i].cos;
+ }
+
+ m_regressionData.data.emplace_back(fast[i].velocity, accelPortion);
+ }
+
+ // Timestep-vs-time plot
+
+ for (size_t i = 0; i < slow.size(); i += slowStep) {
+ if (i > 0) {
+ // If the current timestamp is not in the startTimes array, it is the
+ // during a test and should be included. If it is in the startTimes
+ // array, it is the beginning of a new test and the dt will be inflated.
+ // Therefore we skip those to exclude that dt and effectively reset dt
+ // calculations.
+ if (slow[i].dt > 0_s &&
+ std::find(startTimes.begin(), startTimes.end(), slow[i].timestamp) ==
+ startTimes.end()) {
+ m_timestepData.data.emplace_back(
+ (slow[i].timestamp).value(),
+ units::millisecond_t{slow[i].dt}.value());
+ }
+ }
+ }
+
+ for (size_t i = 0; i < fast.size(); i += fastStep) {
+ if (i > 0) {
+ // If the current timestamp is not in the startTimes array, it is the
+ // during a test and should be included. If it is in the startTimes
+ // array, it is the beginning of a new test and the dt will be inflated.
+ // Therefore we skip those to exclude that dt and effectively reset dt
+ // calculations.
+ if (fast[i].dt > 0_s &&
+ std::find(startTimes.begin(), startTimes.end(), fast[i].timestamp) ==
+ startTimes.end()) {
+ m_timestepData.data.emplace_back(
+ (fast[i].timestamp).value(),
+ units::millisecond_t{fast[i].dt}.value());
+ }
+ }
+ }
+
+ auto minTime =
+ units::math::min(slow.front().timestamp, fast.front().timestamp);
+ m_timestepData.fitLine[0] =
+ ImPlotPoint{minTime.value(), units::millisecond_t{dtMean}.value()};
+
+ auto maxTime = units::math::max(slow.back().timestamp, fast.back().timestamp);
+ m_timestepData.fitLine[1] =
+ ImPlotPoint{maxTime.value(), units::millisecond_t{dtMean}.value()};
+
+ // RMSE = std::sqrt(sum((x_i - x^_i)^2) / N) where sum represents the sum of
+ // all time series points, x_i represents the velocity at a timestep, x^_i
+ // represents the prediction at the timestep, and N represents the number of
+ // points
+ m_RMSE = std::sqrt(simSquaredErrorSum / timeSeriesPoints);
+ m_accelRSquared =
+ 1 - m_RMSE / std::sqrt(squaredVariationSum / timeSeriesPoints);
+ FitPlots();
+}
+
+void AnalyzerPlot::FitPlots() {
+ // Set the "fit" flag to true.
+ m_quasistaticData.fitNextPlot = true;
+ m_dynamicData.fitNextPlot = true;
+ m_regressionData.fitNextPlot = true;
+ m_timestepData.fitNextPlot = true;
+}
+
+double* AnalyzerPlot::GetSimRMSE() {
+ return &m_RMSE;
+}
+
+double* AnalyzerPlot::GetSimRSquared() {
+ return &m_accelRSquared;
+}
+
+static void PlotSimData(std::vector<std::vector<ImPlotPoint>>& data) {
+ for (auto&& pts : data) {
+ ImPlot::SetNextLineStyle(IMPLOT_AUTO_COL, 1.5);
+ ImPlot::PlotLineG("Simulation", Getter, pts.data(), pts.size());
+ }
+}
+
+bool AnalyzerPlot::DisplayPlots() {
+ std::unique_lock lock(m_mutex, std::defer_lock);
+
+ if (!lock.try_lock()) {
+ ImGui::Text("Loading %c",
+ "|/-\\"[static_cast<int>(ImGui::GetTime() / 0.05f) & 3]);
+ return false;
+ }
+
+ ImVec2 plotSize = ImGui::GetContentRegionAvail();
+
+ // Fit two plots horizontally
+ plotSize.x = (plotSize.x - ImGui::GetStyle().ItemSpacing.x) / 2.f;
+
+ // Fit two plots vertically while leaving room for three text boxes
+ const float textBoxHeight = ImGui::GetFontSize() * 1.75;
+ plotSize.y =
+ (plotSize.y - textBoxHeight * 3 - ImGui::GetStyle().ItemSpacing.y) / 2.f;
+
+ m_quasistaticData.Plot("Quasistatic Velocity vs. Time", plotSize,
+ m_velocityLabel.c_str(), m_pointSize);
+ ImGui::SameLine();
+ m_dynamicData.Plot("Dynamic Velocity vs. Time", plotSize,
+ m_velocityLabel.c_str(), m_pointSize);
+
+ m_regressionData.Plot("Acceleration vs. Velocity", plotSize,
+ m_velocityLabel.c_str(), m_velPortionAccelLabel.c_str(),
+ true, true, m_pointSize);
+ ImGui::SameLine();
+ m_timestepData.Plot("Timesteps vs. Time", plotSize, "Time (s)",
+ "Timestep duration (ms)", true, false, m_pointSize,
+ [] { ImPlot::SetupAxisLimits(ImAxis_Y1, 0, 50); });
+
+ return true;
+}
+
+AnalyzerPlot::FilteredDataVsTimePlot::FilteredDataVsTimePlot() {
+ rawData.reserve(kMaxSize);
+ filteredData.reserve(kMaxSize);
+ simData.reserve(kMaxSize);
+}
+
+void AnalyzerPlot::FilteredDataVsTimePlot::Plot(const char* title,
+ const ImVec2& size,
+ const char* yLabel,
+ float pointSize) {
+ // Generate Sim vs Filtered Plot
+ if (fitNextPlot) {
+ ImPlot::SetNextAxesToFit();
+ }
+
+ if (ImPlot::BeginPlot(title, size)) {
+ ImPlot::SetupAxis(ImAxis_X1, "Time (s)", ImPlotAxisFlags_NoGridLines);
+ ImPlot::SetupAxis(ImAxis_Y1, yLabel, ImPlotAxisFlags_NoGridLines);
+ ImPlot::SetupLegend(ImPlotLocation_NorthEast);
+
+ // Plot Raw Data
+ ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
+ ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
+ ImPlot::PlotScatterG("Raw Data", Getter, rawData.data(), rawData.size());
+
+ // Plot Filtered Data after Raw data
+ ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
+ ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
+ ImPlot::PlotScatterG("Filtered Data", Getter, filteredData.data(),
+ filteredData.size());
+
+ // Plot Simulation Data for Velocity Data
+ PlotSimData(simData);
+
+ // Disable constant resizing
+ if (fitNextPlot) {
+ fitNextPlot = false;
+ }
+
+ ImPlot::EndPlot();
+ }
+}
+
+void AnalyzerPlot::FilteredDataVsTimePlot::Clear() {
+ rawData.clear();
+ filteredData.clear();
+ simData.clear();
+}
+
+AnalyzerPlot::DataWithFitLinePlot::DataWithFitLinePlot() {
+ data.reserve(kMaxSize);
+}
+
+void AnalyzerPlot::DataWithFitLinePlot::Plot(const char* title,
+ const ImVec2& size,
+ const char* xLabel,
+ const char* yLabel, bool fitX,
+ bool fitY, float pointSize,
+ std::function<void()> setup) {
+ if (fitNextPlot) {
+ if (fitX && fitY) {
+ ImPlot::SetNextAxesToFit();
+ } else if (fitX && !fitY) {
+ ImPlot::SetNextAxisToFit(ImAxis_X1);
+ } else if (!fitX && fitY) {
+ ImPlot::SetNextAxisToFit(ImAxis_Y1);
+ }
+ }
+
+ if (ImPlot::BeginPlot(title, size)) {
+ setup();
+ ImPlot::SetupAxis(ImAxis_X1, xLabel, ImPlotAxisFlags_NoGridLines);
+ ImPlot::SetupAxis(ImAxis_Y1, yLabel, ImPlotAxisFlags_NoGridLines);
+ ImPlot::SetupLegend(ImPlotLocation_NorthEast);
+
+ // Get a reference to the data that we are plotting.
+ ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
+ ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
+ ImPlot::PlotScatterG("Filtered Data", Getter, data.data(), data.size());
+
+ ImPlot::SetNextLineStyle(IMPLOT_AUTO_COL, 1.5);
+ ImPlot::PlotLineG("Fit", Getter, fitLine.data(), fitLine.size());
+
+ ImPlot::EndPlot();
+
+ if (fitNextPlot) {
+ fitNextPlot = false;
+ }
+ }
+}
+
+void AnalyzerPlot::DataWithFitLinePlot::Clear() {
+ data.clear();
+
+ // Reset line of best fit
+ fitLine[0] = ImPlotPoint{0, 0};
+ fitLine[1] = ImPlotPoint{0, 0};
+}
diff --git a/sysid/src/main/native/cpp/view/JSONConverter.cpp b/sysid/src/main/native/cpp/view/JSONConverter.cpp
new file mode 100644
index 0000000..88eaa6a
--- /dev/null
+++ b/sysid/src/main/native/cpp/view/JSONConverter.cpp
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/analysis/JSONConverter.h"
+#include "sysid/view/JSONConverter.h"
+
+#include <exception>
+
+#include <imgui.h>
+#include <portable-file-dialogs.h>
+#include <wpi/timestamp.h>
+
+#include "sysid/Util.h"
+
+using namespace sysid;
+
+void JSONConverter::DisplayConverter(
+ const char* tooltip,
+ std::function<std::string(std::string_view, wpi::Logger&)> converter) {
+ if (ImGui::Button(tooltip)) {
+ m_opener = std::make_unique<pfd::open_file>(
+ tooltip, "", std::vector<std::string>{"JSON File", SYSID_PFD_JSON_EXT});
+ }
+
+ if (m_opener && m_opener->ready()) {
+ if (!m_opener->result().empty()) {
+ m_location = m_opener->result()[0];
+ try {
+ converter(m_location, m_logger);
+ m_timestamp = wpi::Now() * 1E-6;
+ } catch (const std::exception& e) {
+ ImGui::OpenPopup("Exception Caught!");
+ m_exception = e.what();
+ }
+ }
+ m_opener.reset();
+ }
+
+ if (wpi::Now() * 1E-6 - m_timestamp < 5) {
+ ImGui::SameLine();
+ ImGui::Text("Saved!");
+ }
+
+ // Handle exceptions.
+ ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
+ if (ImGui::BeginPopupModal("Exception Caught!")) {
+ ImGui::PushTextWrapPos(0.0f);
+ ImGui::Text(
+ "An error occurred when parsing the JSON. This most likely means that "
+ "the JSON data is incorrectly formatted.");
+ ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s",
+ m_exception.c_str());
+ ImGui::PopTextWrapPos();
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ }
+ ImGui::EndPopup();
+ }
+}
+
+void JSONConverter::DisplayCSVConvert() {
+ DisplayConverter("Select SysId JSON", sysid::ToCSV);
+}
diff --git a/sysid/src/main/native/cpp/view/Logger.cpp b/sysid/src/main/native/cpp/view/Logger.cpp
new file mode 100644
index 0000000..5e7773d
--- /dev/null
+++ b/sysid/src/main/native/cpp/view/Logger.cpp
@@ -0,0 +1,222 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sysid/view/Logger.h"
+
+#include <exception>
+#include <numbers>
+
+#include <glass/Context.h>
+#include <glass/Storage.h>
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <networktables/NetworkTable.h>
+#include <units/angle.h>
+#include <wpigui.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/view/UILayout.h"
+
+using namespace sysid;
+
+Logger::Logger(glass::Storage& storage, wpi::Logger& logger)
+ : m_logger{logger}, m_ntSettings{"sysid", storage} {
+ wpi::gui::AddEarlyExecute([&] { m_ntSettings.Update(); });
+
+ m_ntSettings.EnableServerOption(false);
+}
+
+void Logger::Display() {
+ // Get the current width of the window. This will be used to scale
+ // our UI elements.
+ float width = ImGui::GetContentRegionAvail().x;
+
+ // Add team number input and apply button for NT connection.
+ m_ntSettings.Display();
+
+ // Reset and clear the internal manager state.
+ ImGui::SameLine();
+ if (ImGui::Button("Reset Telemetry")) {
+ m_settings = TelemetryManager::Settings{};
+ m_manager = std::make_unique<TelemetryManager>(m_settings, m_logger);
+ m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]);
+ }
+
+ // Add NT connection indicator.
+ static ImVec4 kColorDisconnected{1.0f, 0.4f, 0.4f, 1.0f};
+ static ImVec4 kColorConnected{0.2f, 1.0f, 0.2f, 1.0f};
+ ImGui::SameLine();
+ bool ntConnected = nt::NetworkTableInstance::GetDefault().IsConnected();
+ ImGui::TextColored(ntConnected ? kColorConnected : kColorDisconnected,
+ ntConnected ? "NT Connected" : "NT Disconnected");
+
+ // Create a Section for project configuration
+ ImGui::Separator();
+ ImGui::Spacing();
+ ImGui::Text("Project Parameters");
+
+ // Add a dropdown for mechanism type.
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+
+ if (ImGui::Combo("Mechanism", &m_selectedType, kTypes,
+ IM_ARRAYSIZE(kTypes))) {
+ m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]);
+ }
+
+ // Add Dropdown for Units
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+ if (ImGui::Combo("Unit Type", &m_selectedUnit, kUnits,
+ IM_ARRAYSIZE(kUnits))) {
+ m_settings.units = kUnits[m_selectedUnit];
+ }
+
+ sysid::CreateTooltip(
+ "This is the type of units that your gains will be in. For example, if "
+ "you want your flywheel gains in terms of radians, then use the radians "
+ "unit. On the other hand, if your drivetrain will use gains in meters, "
+ "choose meters.");
+
+ // Rotational units have fixed Units per rotations
+ m_isRotationalUnits =
+ (m_settings.units == "Rotations" || m_settings.units == "Degrees" ||
+ m_settings.units == "Radians");
+ if (m_settings.units == "Degrees") {
+ m_settings.unitsPerRotation = 360.0;
+ } else if (m_settings.units == "Radians") {
+ m_settings.unitsPerRotation = 2 * std::numbers::pi;
+ } else if (m_settings.units == "Rotations") {
+ m_settings.unitsPerRotation = 1.0;
+ }
+
+ // Units Per Rotations entry
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
+ ImGui::InputDouble("Units Per Rotation", &m_settings.unitsPerRotation, 0.0f,
+ 0.0f, "%.4f",
+ m_isRotationalUnits ? ImGuiInputTextFlags_ReadOnly
+ : ImGuiInputTextFlags_None);
+ sysid::CreateTooltip(
+ "The logger assumes that the code will be sending recorded motor shaft "
+ "rotations over NetworkTables. This value will then be multiplied by the "
+ "units per rotation to get the measurement in the units you "
+ "specified.\n\nFor non-rotational units (e.g. meters), this value is "
+ "usually the wheel diameter times pi (should not include gearing).");
+ // Create a section for voltage parameters.
+ ImGui::Separator();
+ ImGui::Spacing();
+ ImGui::Text("Voltage Parameters");
+
+ auto CreateVoltageParameters = [this](const char* text, double* data,
+ float min, float max) {
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+ ImGui::PushItemFlag(ImGuiItemFlags_Disabled,
+ m_manager && m_manager->IsActive());
+ float value = static_cast<float>(*data);
+ if (ImGui::SliderFloat(text, &value, min, max, "%.2f")) {
+ *data = value;
+ }
+ ImGui::PopItemFlag();
+ };
+
+ CreateVoltageParameters("Quasistatic Ramp Rate (V/s)",
+ &m_settings.quasistaticRampRate, 0.10f, 0.60f);
+ sysid::CreateTooltip(
+ "This is the rate at which the voltage will increase during the "
+ "quasistatic test.");
+
+ CreateVoltageParameters("Dynamic Step Voltage (V)", &m_settings.stepVoltage,
+ 0.0f, 10.0f);
+ sysid::CreateTooltip(
+ "This is the voltage that will be applied for the "
+ "dynamic voltage (acceleration) tests.");
+
+ // Create a section for tests.
+ ImGui::Separator();
+ ImGui::Spacing();
+ ImGui::Text("Tests");
+
+ auto CreateTest = [this, width](const char* text, const char* itext) {
+ // Display buttons if we have an NT connection.
+ if (nt::NetworkTableInstance::GetDefault().IsConnected()) {
+ // Create button to run tests.
+ if (ImGui::Button(text)) {
+ // Open the warning message.
+ ImGui::OpenPopup("Warning");
+ m_manager->BeginTest(itext);
+ m_opened = text;
+ }
+ if (m_opened == text && ImGui::BeginPopupModal("Warning")) {
+ ImGui::TextWrapped("%s", m_popupText.c_str());
+ if (ImGui::Button(m_manager->IsActive() ? "End Test" : "Close")) {
+ m_manager->EndTest();
+ ImGui::CloseCurrentPopup();
+ m_opened = "";
+ }
+ ImGui::EndPopup();
+ }
+ } else {
+ // Show disabled text when there is no connection.
+ ImGui::TextDisabled("%s", text);
+ }
+
+ // Show whether the tests were run or not.
+ bool run = m_manager->HasRunTest(itext);
+ ImGui::SameLine(width * 0.7);
+ ImGui::Text(run ? "Run" : "Not Run");
+ };
+
+ CreateTest("Quasistatic Forward", "slow-forward");
+ CreateTest("Quasistatic Backward", "slow-backward");
+ CreateTest("Dynamic Forward", "fast-forward");
+ CreateTest("Dynamic Backward", "fast-backward");
+
+ m_manager->RegisterDisplayCallback(
+ [this](const auto& str) { m_popupText = str; });
+
+ // Display the path to where the JSON will be saved and a button to select the
+ // location.
+ ImGui::Separator();
+ ImGui::Spacing();
+ ImGui::Text("Save Location");
+ if (ImGui::Button("Choose")) {
+ m_selector = std::make_unique<pfd::select_folder>("Select Folder");
+ }
+ ImGui::SameLine();
+ ImGui::InputText("##savelocation", &m_jsonLocation,
+ ImGuiInputTextFlags_ReadOnly);
+
+ // Add button to save.
+ ImGui::SameLine(width * 0.9);
+ if (ImGui::Button("Save")) {
+ try {
+ m_manager->SaveJSON(m_jsonLocation);
+ } catch (const std::exception& e) {
+ ImGui::OpenPopup("Exception Caught!");
+ m_exception = e.what();
+ }
+ }
+
+ // Handle exceptions.
+ if (ImGui::BeginPopupModal("Exception Caught!")) {
+ ImGui::Text("%s", m_exception.c_str());
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ }
+ ImGui::EndPopup();
+ }
+
+ // Run periodic methods.
+ SelectDataFolder();
+ m_ntSettings.Update();
+ m_manager->Update();
+}
+
+void Logger::SelectDataFolder() {
+ // If the selector exists and is ready with a result, we can store it.
+ if (m_selector && m_selector->ready()) {
+ m_jsonLocation = m_selector->result();
+ m_selector.reset();
+ }
+}
diff --git a/sysid/src/main/native/include/sysid/Util.h b/sysid/src/main/native/include/sysid/Util.h
new file mode 100644
index 0000000..38cf8b2
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/Util.h
@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <filesystem>
+#include <string>
+#include <string_view>
+
+// The generated AppleScript by portable-file-dialogs for just *.json does not
+// work correctly because it is a uniform type identifier. This means that
+// "public." needs to be prepended to it.
+#ifdef __APPLE__
+#define SYSID_PFD_JSON_EXT "*.public.json"
+#else
+#define SYSID_PFD_JSON_EXT "*.json"
+#endif
+
+#ifdef _WIN32
+#define LAUNCH "gradlew"
+#define LAUNCH_DETACHED "start /b gradlew"
+#define DETACHED_SUFFIX ""
+#else
+#define LAUNCH "./gradlew"
+#define LAUNCH_DETACHED "./gradlew"
+#define DETACHED_SUFFIX "&"
+#endif
+
+// Based on https://gcc.gnu.org/onlinedocs/cpp/Stringizing.html
+#define EXPAND_STRINGIZE(s) STRINGIZE(s)
+#define STRINGIZE(s) #s
+
+namespace sysid {
+static constexpr const char* kUnits[] = {"Meters", "Feet", "Inches",
+ "Radians", "Rotations", "Degrees"};
+
+/**
+ * Displays a tooltip beside the widget that this method is called after with
+ * the provided text.
+ *
+ * @param text The text to show in the tooltip.
+ */
+void CreateTooltip(const char* text);
+
+/**
+ * Utility function to launch an error popup if an exception is detected.
+ *
+ * @param isError True if an exception is detected
+ * @param errorMessage The error message associated with the exception
+ */
+void CreateErrorPopup(bool& isError, std::string_view errorMessage);
+
+/**
+ * Returns the abbreviation for the unit.
+ *
+ * @param unit The unit to return the abbreviation for.
+ * @return The abbreviation for the unit.
+ */
+std::string_view GetAbbreviation(std::string_view unit);
+
+/**
+ * Saves a file with the provided contents to a specified location.
+ *
+ * @param contents The file contents.
+ * @param path The file location.
+ */
+void SaveFile(std::string_view contents, const std::filesystem::path& path);
+
+/**
+ * Concatenates all the arrays passed as arguments and returns the result.
+ *
+ * @tparam Type The array element type.
+ * @tparam Sizes The array sizes.
+ * @param arrays Parameter pack of arrays to concatenate.
+ */
+template <typename Type, size_t... Sizes>
+constexpr auto ArrayConcat(const std::array<Type, Sizes>&... arrays) {
+ std::array<Type, (Sizes + ...)> result;
+ size_t index = 0;
+
+ ((std::copy_n(arrays.begin(), Sizes, result.begin() + index), index += Sizes),
+ ...);
+
+ return result;
+}
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
new file mode 100644
index 0000000..d572578
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
@@ -0,0 +1,358 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <exception>
+#include <limits>
+#include <numeric>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <tuple>
+#include <vector>
+
+#include <units/time.h>
+#include <wpi/Logger.h>
+#include <wpi/json.h>
+
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/FeedbackAnalysis.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+#include "sysid/analysis/FeedforwardAnalysis.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+
+/**
+ * Manages analysis of data. Each instance of this class represents a JSON file
+ * that is read from storage.
+ */
+class AnalysisManager {
+ public:
+ /**
+ * Represents settings for an instance of the analysis manager. This contains
+ * information about the feedback controller preset, loop type, motion
+ * threshold, acceleration window size, LQR parameters, and the selected
+ * dataset.
+ */
+ struct Settings {
+ enum class DrivetrainDataset { kCombined = 0, kLeft = 1, kRight = 2 };
+ /**
+ * The feedback controller preset used to calculate gains.
+ */
+ FeedbackControllerPreset preset = presets::kDefault;
+
+ /**
+ * The feedback controller loop type (position or velocity).
+ */
+ FeedbackControllerLoopType type = FeedbackControllerLoopType::kVelocity;
+
+ /**
+ * LQR parameters used for feedback gain calculation.
+ */
+ LQRParameters lqr{1, 1.5, 7};
+
+ /**
+ * The motion threshold (units/s) for trimming quasistatic test data.
+ */
+ double motionThreshold = 0.2;
+
+ /**
+ * The window size for the median filter.
+ */
+ int medianWindow = 1;
+
+ /**
+ * The duration of the dynamic test that should be considered. A value of
+ * zero indicates it needs to be set to the default.
+ */
+ units::second_t stepTestDuration = 0_s;
+
+ /**
+ * The conversion factor of counts per revolution.
+ */
+ int cpr = 1440;
+
+ /**
+ * The conversion factor of gearing.
+ */
+ double gearing = 1;
+
+ /**
+ * Whether or not the gains should be in the encoder's units (mainly for use
+ * in a smart motor controller).
+ */
+ bool convertGainsToEncTicks = false;
+
+ DrivetrainDataset dataset = DrivetrainDataset::kCombined;
+ };
+
+ /**
+ * Stores feedforward.
+ */
+ struct FeedforwardGains {
+ /**
+ * Stores the Feedforward gains.
+ */
+ std::tuple<std::vector<double>, double, double> ffGains;
+
+ /**
+ * Stores the trackwidth for angular drivetrain tests.
+ */
+ std::optional<double> trackWidth;
+ };
+
+ /**
+ * Exception for File Reading Errors.
+ */
+ struct FileReadingError : public std::exception {
+ /**
+ * Creates a FileReadingError object
+ *
+ * @param path The path of the file attempted to open
+ */
+ explicit FileReadingError(std::string_view path) {
+ msg = fmt::format("Unable to read: {}", path);
+ }
+
+ /**
+ * The path of the file that was opened.
+ */
+ std::string msg;
+ const char* what() const noexcept override { return msg.c_str(); }
+ };
+
+ /**
+ * The keys (which contain sysid data) that are in the JSON to analyze.
+ */
+ static constexpr const char* kJsonDataKeys[] = {
+ "slow-forward", "slow-backward", "fast-forward", "fast-backward"};
+
+ /**
+ * Concatenates a list of vectors. The contents of the source vectors are
+ * copied (not moved) into the new vector. Also sorts the datapoints by
+ * timestamp to assist with future simulation.
+ *
+ * @param sources The source vectors.
+ * @return The concatenated vector
+ */
+ template <typename... Sources>
+ static std::vector<PreparedData> DataConcat(const Sources&... sources) {
+ std::vector<PreparedData> result;
+ (result.insert(result.end(), sources.begin(), sources.end()), ...);
+
+ // Sort data by timestamp to remove the possibility of negative dts in
+ // future simulations.
+ std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) {
+ return a.timestamp < b.timestamp;
+ });
+
+ return result;
+ }
+
+ /**
+ * Constructs an instance of the analysis manager for theoretical analysis,
+ * containing settings and gains but no data.
+ *
+ * @param settings The settings for this instance of the analysis manager.
+ * @param logger The logger instance to use for log data.
+ */
+ AnalysisManager(Settings& settings, wpi::Logger& logger);
+
+ /**
+ * Constructs an instance of the analysis manager with the given path (to the
+ * JSON) and analysis manager settings.
+ *
+ * @param path The path to the JSON containing the sysid data.
+ * @param settings The settings for this instance of the analysis manager.
+ * @param logger The logger instance to use for log data.
+ */
+ AnalysisManager(std::string_view path, Settings& settings,
+ wpi::Logger& logger);
+
+ /**
+ * Prepares data from the JSON and stores the output in Storage member
+ * variables.
+ */
+ void PrepareData();
+
+ /**
+ * Calculates the gains with the latest data (from the pointers in the
+ * settings struct that this instance was constructed with).
+ *
+ * @return The latest feedforward gains and trackwidth (if needed).
+ */
+ FeedforwardGains CalculateFeedforward();
+
+ /**
+ * Calculates feedback gains from the given feedforward gains.
+ *
+ * @param ff The feedforward gains.
+ * @return The calculated feedback gains.
+ */
+ FeedbackGains CalculateFeedback(std::vector<double> ff);
+
+ /**
+ * Overrides the units in the JSON with the user-provided ones.
+ *
+ * @param unit The unit to output gains in.
+ * @param unitsPerRotation The conversion factor between rotations and the
+ * selected unit.
+ */
+ void OverrideUnits(std::string_view unit, double unitsPerRotation);
+
+ /**
+ * Resets the units back to those defined in the JSON.
+ */
+ void ResetUnitsFromJSON();
+
+ /**
+ * Returns the analysis type of the current instance (read from the JSON).
+ *
+ * @return The analysis type.
+ */
+ const AnalysisType& GetAnalysisType() const { return m_type; }
+
+ /**
+ * Returns the units of analysis.
+ *
+ * @return The units of analysis.
+ */
+ std::string_view GetUnit() const { return m_unit; }
+
+ /**
+ * Returns the factor (a.k.a. units per rotation) for analysis.
+ *
+ * @return The factor (a.k.a. units per rotation) for analysis.
+ */
+ double GetFactor() const { return m_factor; }
+
+ /**
+ * Returns a reference to the iterator of the currently selected raw datset.
+ * Unfortunately, due to ImPlot internals, the reference cannot be const so
+ * the user should be careful not to change any data.
+ *
+ * @return A reference to the raw internal data.
+ */
+ Storage& GetRawData() {
+ return m_rawDataset[static_cast<int>(m_settings.dataset)];
+ }
+
+ /**
+ * Returns a reference to the iterator of the currently selected filtered
+ * datset. Unfortunately, due to ImPlot internals, the reference cannot be
+ * const so the user should be careful not to change any data.
+ *
+ * @return A reference to the filtered internal data.
+ */
+ Storage& GetFilteredData() {
+ return m_filteredDataset[static_cast<int>(m_settings.dataset)];
+ }
+
+ /**
+ * Returns the original dataset.
+ *
+ * @return The original (untouched) dataset
+ */
+ Storage& GetOriginalData() {
+ return m_originalDataset[static_cast<int>(m_settings.dataset)];
+ }
+
+ /**
+ * Returns the minimum duration of the Step Voltage Test of the currently
+ * stored data.
+ *
+ * @return The minimum step test duration.
+ */
+ units::second_t GetMinStepTime() const { return m_minStepTime; }
+
+ /**
+ * Returns the maximum duration of the Step Voltage Test of the currently
+ * stored data.
+ *
+ * @return Maximum step test duration
+ */
+ units::second_t GetMaxStepTime() const { return m_maxStepTime; }
+
+ /**
+ * Returns the estimated time delay of the measured position, including
+ * CAN delays.
+ *
+ * @return Position delay in milliseconds
+ */
+ units::millisecond_t GetPositionDelay() const {
+ return std::accumulate(m_positionDelays.begin(), m_positionDelays.end(),
+ 0_s) /
+ m_positionDelays.size();
+ }
+
+ /**
+ * Returns the estimated time delay of the measured velocity, including
+ * CAN delays.
+ *
+ * @return Velocity delay in milliseconds
+ */
+ units::millisecond_t GetVelocityDelay() const {
+ return std::accumulate(m_velocityDelays.begin(), m_velocityDelays.end(),
+ 0_s) /
+ m_positionDelays.size();
+ }
+
+ /**
+ * Returns the different start times of the recorded tests.
+ *
+ * @return The start times for each test
+ */
+ const std::array<units::second_t, 4>& GetStartTimes() const {
+ return m_startTimes;
+ }
+
+ bool HasData() const {
+ return !m_originalDataset[static_cast<int>(
+ Settings::DrivetrainDataset::kCombined)]
+ .empty();
+ }
+
+ private:
+ wpi::Logger& m_logger;
+
+ // This is used to store the various datasets (i.e. Combined, Forward,
+ // Backward, etc.)
+ wpi::json m_json;
+
+ std::array<Storage, 3> m_originalDataset;
+ std::array<Storage, 3> m_rawDataset;
+ std::array<Storage, 3> m_filteredDataset;
+
+ // Stores the various start times of the different tests.
+ std::array<units::second_t, 4> m_startTimes;
+
+ // The settings for this instance. This contains pointers to the feedback
+ // controller preset, LQR parameters, acceleration window size, etc.
+ Settings& m_settings;
+
+ // Miscellaneous data from the JSON -- the analysis type, the units, and the
+ // units per rotation.
+ AnalysisType m_type;
+ std::string m_unit;
+ double m_factor;
+
+ units::second_t m_minStepTime{0};
+ units::second_t m_maxStepTime{std::numeric_limits<double>::infinity()};
+ std::vector<units::second_t> m_positionDelays;
+ std::vector<units::second_t> m_velocityDelays;
+
+ // Stores an optional track width if we are doing the drivetrain angular test.
+ std::optional<double> m_trackWidth;
+
+ void PrepareGeneralData();
+
+ void PrepareAngularDrivetrainData();
+
+ void PrepareLinearDrivetrainData();
+};
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/AnalysisType.h b/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
new file mode 100644
index 0000000..7feedb3
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cstddef>
+#include <string_view>
+
+namespace sysid {
+
+/**
+ * Stores the type of Analysis and relevant information about the analysis.
+ */
+struct AnalysisType {
+ /**
+ * The number of independent variables for feedforward analysis.
+ */
+ size_t independentVariables;
+
+ /**
+ * The number of fields in the raw data within the mechanism's JSON.
+ */
+ size_t rawDataSize;
+
+ /**
+ * Display name for the analysis type.
+ */
+ const char* name;
+
+ /**
+ * Compares equality of two AnalysisType structs.
+ *
+ * @param rhs Another AnalysisType
+ * @return True if the two analysis types are equal
+ */
+ constexpr bool operator==(const AnalysisType& rhs) const {
+ return std::string_view{name} == rhs.name &&
+ independentVariables == rhs.independentVariables &&
+ rawDataSize == rhs.rawDataSize;
+ }
+
+ /**
+ * Compares inequality of two AnalysisType structs.
+ *
+ * @param rhs Another AnalysisType
+ * @return True if the two analysis types are not equal
+ */
+ constexpr bool operator!=(const AnalysisType& rhs) const {
+ return !operator==(rhs);
+ }
+};
+
+namespace analysis {
+constexpr AnalysisType kDrivetrain{3, 9, "Drivetrain"};
+constexpr AnalysisType kDrivetrainAngular{3, 9, "Drivetrain (Angular)"};
+constexpr AnalysisType kElevator{4, 4, "Elevator"};
+constexpr AnalysisType kArm{5, 4, "Arm"};
+constexpr AnalysisType kSimple{3, 4, "Simple"};
+
+AnalysisType FromName(std::string_view name);
+} // namespace analysis
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/ArmSim.h b/sysid/src/main/native/include/sysid/analysis/ArmSim.h
new file mode 100644
index 0000000..af6c10e
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/ArmSim.h
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace sysid {
+/**
+ * Simulation of an Arm mechanism based off of a model from SysId Feedforward
+ * gains.
+ */
+class ArmSim {
+ public:
+ /**
+ * @param Ks Static friction gain.
+ * @param Kv Velocity gain.
+ * @param Ka Acceleration gain.
+ * @param Kg Gravity cosine gain.
+ * @param offset Arm position offset.
+ * @param initialPosition Initial arm position.
+ * @param initialVelocity Initial arm velocity.
+ */
+ ArmSim(double Ks, double Kv, double Ka, double Kg, double offset = 0.0,
+ double initialPosition = 0.0, double initialVelocity = 0.0);
+
+ /**
+ * Simulates affine state-space system:
+ * dx/dt = Ax + Bu + c sgn(x) + d cos(theta)
+ * forward dt seconds.
+ *
+ * @param voltage Voltage to apply over the timestep.
+ * @param dt Sammple period.
+ */
+ void Update(units::volt_t voltage, units::second_t dt);
+
+ /**
+ * Returns the position.
+ *
+ * @return The current position
+ */
+ double GetPosition() const;
+
+ /**
+ * Returns the velocity.
+ *
+ * @return The current velocity
+ */
+ double GetVelocity() const;
+
+ /**
+ * Returns the acceleration for the current state and given input.
+ *
+ * @param voltage The voltage that is being applied to the mechanism / input
+ * @return The acceleration given the state and input
+ */
+ double GetAcceleration(units::volt_t voltage) const;
+
+ /**
+ * Resets model position and velocity.
+ *
+ * @param position The position the mechanism should be reset to
+ * @param velocity The velocity the mechanism should be reset to
+ */
+ void Reset(double position = 0.0, double velocity = 0.0);
+
+ private:
+ Eigen::Matrix<double, 1, 1> m_A;
+ Eigen::Matrix<double, 1, 1> m_B;
+ Eigen::Vector<double, 1> m_c;
+ Eigen::Vector<double, 1> m_d;
+ Eigen::Vector<double, 2> m_x;
+ double m_offset;
+};
+
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h b/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h
new file mode 100644
index 0000000..2d0c5f6
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h
@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace sysid {
+/**
+ * Simulation of an Elevator mechanism based off of a model from SysId
+ * Feedforward gains.
+ */
+class ElevatorSim {
+ public:
+ /**
+ * @param Ks Static friction gain.
+ * @param Kv Velocity gain.
+ * @param Ka Acceleration gain.
+ * @param Kg Gravity gain.
+ * @param initialPosition Initial elevator position.
+ * @param initialVelocity Initial elevator velocity.
+ */
+ ElevatorSim(double Ks, double Kv, double Ka, double Kg,
+ double initialPosition = 0.0, double initialVelocity = 0.0);
+
+ /**
+ * Simulates affine state-space system dx/dt = Ax + Bu + c sgn(x) + d forward
+ * dt seconds.
+ *
+ * @param voltage Voltage to apply over the timestep.
+ * @param dt Sammple period.
+ */
+ void Update(units::volt_t voltage, units::second_t dt);
+
+ /**
+ * Returns the position.
+ *
+ * @return The current position
+ */
+ double GetPosition() const;
+
+ /**
+ * Returns the velocity.
+ *
+ * @return The current velocity
+ */
+ double GetVelocity() const;
+
+ /**
+ * Returns the acceleration for the current state and given input.
+ *
+ * @param voltage The voltage that is being applied to the mechanism / input
+ * @return The acceleration given the state and input
+ */
+ double GetAcceleration(units::volt_t voltage) const;
+
+ /**
+ * Resets model position and velocity.
+ *
+ * @param position The position the mechanism should be reset to
+ * @param velocity The velocity the mechanism should be reset to
+ */
+ void Reset(double position = 0.0, double velocity = 0.0);
+
+ private:
+ Eigen::Matrix<double, 2, 2> m_A;
+ Eigen::Matrix<double, 2, 1> m_B;
+ Eigen::Vector<double, 2> m_c;
+ Eigen::Vector<double, 2> m_d;
+ Eigen::Vector<double, 2> m_x;
+};
+
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h b/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h
new file mode 100644
index 0000000..51754a2
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace sysid {
+
+struct FeedbackControllerPreset;
+
+/**
+ * Represents parameters used to calculate optimal feedback gains using a
+ * linear-quadratic regulator (LQR).
+ */
+struct LQRParameters {
+ /**
+ * The maximum allowable deviation in position.
+ */
+ double qp;
+
+ /**
+ * The maximum allowable deviation in velocity.
+ */
+ double qv;
+
+ /**
+ * The maximum allowable control effort.
+ */
+ double r;
+};
+
+/**
+ * Stores feedback controller gains.
+ */
+struct FeedbackGains {
+ /**
+ * The calculated Proportional gain
+ */
+ double Kp;
+
+ /**
+ * The calculated Derivative gain
+ */
+ double Kd;
+};
+
+/**
+ * Calculates position feedback gains for the given controller preset, LQR
+ * controller gain parameters and feedforward gains.
+ *
+ * @param preset The feedback controller preset.
+ * @param params The parameters for calculating optimal feedback
+ * gains.
+ * @param Kv Velocity feedforward gain.
+ * @param Ka Acceleration feedforward gain.
+ * @param encFactor The factor to convert the gains from output units to
+ * encoder units. This is usually encoder EPR * gearing
+ * * units per rotation.
+ */
+FeedbackGains CalculatePositionFeedbackGains(
+ const FeedbackControllerPreset& preset, const LQRParameters& params,
+ double Kv, double Ka, double encFactor = 1.0);
+
+/**
+ * Calculates velocity feedback gains for the given controller preset, LQR
+ * controller gain parameters and feedforward gains.
+ *
+ * @param preset The feedback controller preset.
+ * @param params The parameters for calculating optimal feedback
+ * gains.
+ * @param Kv Velocity feedforward gain.
+ * @param Ka Acceleration feedforward gain.
+ * @param encFactor The factor to convert the gains from output units to
+ * encoder units. This is usually encoder EPR * gearing
+ * * units per rotation.
+ */
+FeedbackGains CalculateVelocityFeedbackGains(
+ const FeedbackControllerPreset& preset, const LQRParameters& params,
+ double Kv, double Ka, double encFactor = 1.0);
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h b/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
new file mode 100644
index 0000000..4b13c6c
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
@@ -0,0 +1,164 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <units/time.h>
+
+namespace sysid {
+/**
+ * Represents a preset for a specific feedback controller. This includes info
+ * about the max controller output, the controller period, whether the gains
+ * are time-normalized, and whether there are measurement delays from sensors or
+ * onboard filtering.
+ */
+struct FeedbackControllerPreset {
+ /**
+ * The conversion factor between volts and the final controller output.
+ */
+ double outputConversionFactor;
+
+ /**
+ * The conversion factor for using controller output for velocity gains. This
+ * is necessary as some controllers do velocity controls with different time
+ * units.
+ */
+ double outputVelocityTimeFactor;
+
+ /**
+ * The period at which the controller runs.
+ */
+ units::millisecond_t period;
+
+ /**
+ * Whether the controller gains are time-normalized.
+ */
+ bool normalized;
+
+ /**
+ * The measurement delay in the encoder measurements.
+ */
+ units::millisecond_t measurementDelay;
+
+ /**
+ * Checks equality between two feedback controller presets.
+ *
+ * @param rhs Another FeedbackControllerPreset
+ * @return If the two presets are equal
+ */
+ constexpr bool operator==(const FeedbackControllerPreset& rhs) const {
+ return outputConversionFactor == rhs.outputConversionFactor &&
+ outputVelocityTimeFactor == rhs.outputVelocityTimeFactor &&
+ period == rhs.period && normalized == rhs.normalized &&
+ measurementDelay == rhs.measurementDelay;
+ }
+
+ /**
+ * Checks inequality between two feedback controller presets.
+ *
+ * @param rhs Another FeedbackControllerPreset
+ * @return If the two presets are not equal
+ */
+ constexpr bool operator!=(const FeedbackControllerPreset& rhs) const {
+ return !operator==(rhs);
+ }
+};
+
+/**
+ * The loop type for the feedback controller.
+ */
+enum class FeedbackControllerLoopType { kPosition, kVelocity };
+
+namespace presets {
+constexpr FeedbackControllerPreset kDefault{1.0, 1.0, 20_ms, true, 0_s};
+
+constexpr FeedbackControllerPreset kWPILibNew{kDefault};
+constexpr FeedbackControllerPreset kWPILibOld{1.0 / 12.0, 1.0, 50_ms, false,
+ 0_s};
+
+// Measurement delay from a moving average filter:
+//
+// A moving average filter with a window size of N is an FIR filter with N taps.
+// The average delay of a moving average filter with N taps and a period between
+// samples of T is (N - 1)/2 T.
+//
+// Proof:
+// N taps with delays of 0 .. (N - 1) T
+//
+// average delay = (sum 0 .. N - 1) / N T
+// = (sum 1 .. N - 1) / N T
+//
+// note: sum 1 .. n = n(n + 1) / 2
+//
+// = (N - 1)((N - 1) + 1) / (2N) T
+// = (N - 1)N / (2N) T
+// = (N - 1)/2 T
+
+// Measurement delay from a backward finite difference:
+//
+// Velocities calculated via a backward finite difference with a period of T
+// look like:
+//
+// velocity = (position at timestep k - position at timestep k-1) / T
+//
+// The average delay is T / 2.
+//
+// Proof:
+// average delay = (0 ms + T) / 2
+// = T / 2
+
+/**
+ * https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#changing-velocity-measurement-parameters
+ *
+ * Backward finite difference delay = 100 ms / 2 = 50 ms.
+ *
+ * 64-tap moving average delay = (64 - 1) / 2 * 1 ms = 31.5 ms.
+ *
+ * Total delay = 50 ms + 31.5 ms = 81.5 ms.
+ */
+constexpr FeedbackControllerPreset kCTRECANCoder{1.0 / 12.0, 60.0, 1_ms, true,
+ 81.5_ms};
+constexpr FeedbackControllerPreset kCTREDefault{1023.0 / 12.0, 0.1, 1_ms, false,
+ 81.5_ms};
+/**
+ * https://api.ctr-electronics.com/phoenixpro/release/cpp/classctre_1_1phoenixpro_1_1hardware_1_1core_1_1_core_c_a_ncoder.html#a718a1a214b58d3c4543e88e3cb51ade5
+ *
+ * Phoenix Pro uses standard units and Voltage output. This means the output
+ * is 1.0, time factor is 1.0, and closed loop operates at 1 millisecond. All
+ * Pro devices make use of Kalman filters default-tuned to lowest latency, which
+ * in testing is roughly 1 millisecond
+ */
+constexpr FeedbackControllerPreset kCTREProDefault{1.0, 1.0, 1_ms, true, 1_ms};
+
+/**
+ * https://github.com/wpilibsuite/sysid/issues/258#issuecomment-1010658237
+ *
+ * 8-sample moving average with 32 ms between samples.
+ *
+ * Total delay = 8-tap moving average delay = (8 - 1) / 2 * 32 ms = 112 ms.
+ */
+constexpr FeedbackControllerPreset kREVNEOBuiltIn{1.0 / 12.0, 60.0, 1_ms, false,
+ 112_ms};
+
+/**
+ * https://www.revrobotics.com/content/sw/max/sw-docs/cpp/classrev_1_1_c_a_n_encoder.html#a7e6ce792bc0c0558fb944771df572e6a
+ *
+ * Backward finite difference delay = 100 ms / 2 = 50 ms.
+ *
+ * 64-tap moving average delay = (64 - 1) / 2 * 1 ms = 31.5 ms.
+ *
+ * Total delay = 50 ms + 31.5 ms = 81.5 ms.
+ */
+constexpr FeedbackControllerPreset kREVNonNEO{1.0 / 12.0, 60.0, 1_ms, false,
+ 81.5_ms};
+
+/**
+ * https://github.com/wpilibsuite/sysid/pull/138#issuecomment-841734229
+ *
+ * Backward finite difference delay = 10 ms / 2 = 5 ms.
+ */
+constexpr FeedbackControllerPreset kVenom{4096.0 / 12.0, 60.0, 1_ms, false,
+ 5_ms};
+} // namespace presets
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
new file mode 100644
index 0000000..fc9e47c
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <tuple>
+#include <vector>
+
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+
+/**
+ * Calculates feedforward gains given the data and the type of analysis to
+ * perform.
+ *
+ * @return Tuple containing the coefficients of the analysis along with the
+ * r-squared (coefficient of determination) and RMSE (standard deviation
+ * of the residuals) of the fit.
+ */
+std::tuple<std::vector<double>, double, double> CalculateFeedforwardGains(
+ const Storage& data, const AnalysisType& type);
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
new file mode 100644
index 0000000..9030c00
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
@@ -0,0 +1,210 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <exception>
+#include <functional>
+#include <string>
+#include <string_view>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+#include <frc/filter/LinearFilter.h>
+#include <units/time.h>
+#include <wpi/StringMap.h>
+#include <wpi/array.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+
+constexpr int kNoiseMeanWindow = 9;
+
+/**
+ * Exception for Invalid Data Errors in which we can't pin the cause of error to
+ * any one specific setting of the GUI.
+ */
+struct InvalidDataError : public std::exception {
+ /**
+ * Creates an InvalidDataError Exception. It adds additional steps after the
+ * initial error message to inform users in the ways that they could fix their
+ * data.
+ *
+ * @param message The error message
+ */
+ explicit InvalidDataError(std::string_view message) {
+ m_message = fmt::format(
+ "{}. Please verify that your units and data is reasonable and then "
+ "adjust your motion threshold, test duration, and/or window size to "
+ "try to fix this issue.",
+ message);
+ }
+
+ /**
+ * Stores the error message
+ */
+ std::string m_message;
+ const char* what() const noexcept override { return m_message.c_str(); }
+};
+
+/**
+ * Exception for Quasistatic Data being completely removed.
+ */
+struct NoQuasistaticDataError : public std::exception {
+ const char* what() const noexcept override {
+ return "Quasistatic test trimming removed all data. Please adjust your "
+ "motion threshold and double check "
+ "your units and test data to make sure that the robot is reporting "
+ "reasonable values.";
+ }
+};
+
+/**
+ * Exception for Dynamic Data being completely removed.
+ */
+struct NoDynamicDataError : public std::exception {
+ const char* what() const noexcept override {
+ return "Dynamic test trimming removed all data. Please adjust your test "
+ "duration and double check "
+ "your units and test data to make sure that the robot is reporting "
+ "reasonable values.";
+ }
+};
+
+/**
+ * Calculates the expected acceleration noise to be used as the floor of the
+ * Voltage Trim. This is done by taking the standard deviation from the moving
+ * average values of each point.
+ *
+ * @param data the prepared data vector containing acceleration data
+ * @param window the size of the window for the moving average
+ * @param accessorFunction a function that accesses the desired data from the
+ * PreparedData struct.
+ * @return The expected acceleration noise
+ */
+double GetNoiseFloor(
+ const std::vector<PreparedData>& data, int window,
+ std::function<double(const PreparedData&)> accessorFunction);
+
+/**
+ * Reduces noise in velocity data by applying a median filter.
+ *
+ * @tparam S The size of the raw data array
+ * @tparam Velocity The index of the velocity entry in the raw data.
+ * @param data the vector of arrays representing sysid data (must contain
+ * velocity data)
+ * @param window the size of the window of the median filter (must be odd)
+ */
+void ApplyMedianFilter(std::vector<PreparedData>* data, int window);
+
+/**
+ * Trims the step voltage data to discard all points before the maximum
+ * acceleration and after reaching stead-state velocity. Also trims the end of
+ * the test based off of user specified test durations, but it will determine a
+ * default duration if the requested duration is less than the minimum step test
+ * duration.
+ *
+ * @param data A pointer to the step voltage data.
+ * @param settings A pointer to the settings of an analysis manager object.
+ * @param minStepTime The current minimum step test duration as one of the
+ * trimming procedures will remove this amount from the start
+ * of the test.
+ * @param maxStepTime The maximum step test duration.
+ * @return The updated minimum step test duration.
+ */
+std::tuple<units::second_t, units::second_t, units::second_t>
+TrimStepVoltageData(std::vector<PreparedData>* data,
+ AnalysisManager::Settings* settings,
+ units::second_t minStepTime, units::second_t maxStepTime);
+
+/**
+ * Compute the mean time delta of the given data.
+ *
+ * @param data A reference to all of the collected PreparedData
+ * @return The mean time delta for all the data points
+ */
+units::second_t GetMeanTimeDelta(const std::vector<PreparedData>& data);
+
+/**
+ * Compute the mean time delta of the given data.
+ *
+ * @param data A reference to all of the collected PreparedData
+ * @return The mean time delta for all the data points
+ */
+units::second_t GetMeanTimeDelta(const Storage& data);
+
+/**
+ * Creates a central finite difference filter that computes the nth
+ * derivative of the input given the specified number of samples.
+ *
+ * Since this requires data from the future, it should only be used in offline
+ * filtering scenarios.
+ *
+ * For example, a first derivative filter that uses two samples and a sample
+ * period of 20 ms would be
+ *
+ * <pre><code>
+ * CentralFiniteDifference<1, 2>(20_ms);
+ * </code></pre>
+ *
+ * @tparam Derivative The order of the derivative to compute.
+ * @tparam Samples The number of samples to use to compute the given
+ * derivative. This must be odd and one more than the order
+ * of derivative or higher.
+ * @param period The period in seconds between samples taken by the user.
+ */
+template <int Derivative, int Samples>
+frc::LinearFilter<double> CentralFiniteDifference(units::second_t period) {
+ static_assert(Samples % 2 != 0, "Number of samples must be odd.");
+
+ // Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
+ wpi::array<int, Samples> stencil{wpi::empty_array};
+ for (int i = 0; i < Samples; ++i) {
+ stencil[i] = -(Samples - 1) / 2 + i;
+ }
+
+ return frc::LinearFilter<double>::FiniteDifference<Derivative, Samples>(
+ stencil, period);
+}
+
+/**
+ * Trims the quasistatic tests, applies a median filter to the velocity data,
+ * calculates acceleration and cosine (arm only) data, and trims the dynamic
+ * tests.
+ *
+ * @param data A pointer to a data vector recently created by the
+ * ConvertToPrepared method
+ * @param settings A reference to the analysis settings
+ * @param positionDelays A reference to the vector of computed position signal
+ * delays.
+ * @param velocityDelays A reference to the vector of computed velocity signal
+ * delays.
+ * @param minStepTime A reference to the minimum dynamic test duration as one of
+ * the trimming procedures will remove this amount from the
+ * start of the test.
+ * @param maxStepTime A reference to the maximum dynamic test duration
+ * @param unit The angular unit that the arm test is in (only for calculating
+ * cosine data)
+ */
+void InitialTrimAndFilter(wpi::StringMap<std::vector<PreparedData>>* data,
+ AnalysisManager::Settings* settings,
+ std::vector<units::second_t>& positionDelays,
+ std::vector<units::second_t>& velocityDelays,
+ units::second_t& minStepTime,
+ units::second_t& maxStepTime,
+ std::string_view unit = "");
+
+/**
+ * Removes all points with acceleration = 0.
+ *
+ * @param data A pointer to a PreparedData vector
+ */
+void AccelFilter(wpi::StringMap<std::vector<PreparedData>>* data);
+
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/JSONConverter.h b/sysid/src/main/native/include/sysid/analysis/JSONConverter.h
new file mode 100644
index 0000000..7581d25
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/JSONConverter.h
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <wpi/Logger.h>
+
+namespace sysid {
+/**
+ * Converts a JSON from the old frc-characterization format to the new sysid
+ * format.
+ *
+ * @param path The path to the old JSON.
+ * @param logger The logger instance for log messages.
+ * @return The full file path of the newly saved JSON.
+ */
+std::string ConvertJSON(std::string_view path, wpi::Logger& logger);
+
+std::string ToCSV(std::string_view path, wpi::Logger& logger);
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/OLS.h b/sysid/src/main/native/include/sysid/analysis/OLS.h
new file mode 100644
index 0000000..cf97904
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/OLS.h
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cstddef>
+#include <tuple>
+#include <vector>
+
+#include <Eigen/Core>
+
+namespace sysid {
+
+/**
+ * Performs ordinary least squares multiple regression on the provided data and
+ * returns a vector of coefficients along with the r-squared (coefficient of
+ * determination) and RMSE (stardard deviation of the residuals) of the fit.
+ *
+ * @param X The independent data in y = Xβ.
+ * @param y The dependent data in y = Xβ.
+ */
+std::tuple<std::vector<double>, double, double> OLS(const Eigen::MatrixXd& X,
+ const Eigen::VectorXd& y);
+
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h b/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h
new file mode 100644
index 0000000..a452c64
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h
@@ -0,0 +1,74 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace sysid {
+/**
+ * Simulation of a Simple Motor mechanism based off of a model from SysId
+ * Feedforward gains.
+ */
+class SimpleMotorSim {
+ public:
+ /**
+ * @param Ks Static friction gain.
+ * @param Kv Velocity gain.
+ * @param Ka Acceleration gain.
+ * @param initialPosition Initial flywheel position.
+ * @param initialVelocity Initial flywheel velocity.
+ */
+ SimpleMotorSim(double Ks, double Kv, double Ka, double initialPosition = 0.0,
+ double initialVelocity = 0.0);
+
+ /**
+ * Simulates affine state-space system dx/dt = Ax + Bu + c sgn(x) forward dt
+ * seconds.
+ *
+ * @param voltage Voltage to apply over the timestep.
+ * @param dt Sammple period.
+ */
+ void Update(units::volt_t voltage, units::second_t dt);
+
+ /**
+ * Returns the position.
+ *
+ * @return The current position
+ */
+ double GetPosition() const;
+
+ /**
+ * Returns the velocity.
+ *
+ * @return The current velocity
+ */
+ double GetVelocity() const;
+
+ /**
+ * Returns the acceleration for the current state and given input.
+ *
+ * @param voltage The voltage that is being applied to the mechanism / input
+ * @return The acceleration given the state and input
+ */
+ double GetAcceleration(units::volt_t voltage) const;
+
+ /**
+ * Resets model position and velocity.
+ *
+ * @param position The position the mechanism should be reset to
+ * @param velocity The velocity the mechanism should be reset to
+ */
+ void Reset(double position = 0.0, double velocity = 0.0);
+
+ private:
+ Eigen::Matrix<double, 2, 2> m_A;
+ Eigen::Matrix<double, 2, 1> m_B;
+ Eigen::Vector<double, 2> m_c;
+ Eigen::Vector<double, 2> m_x;
+};
+
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/Storage.h b/sysid/src/main/native/include/sysid/analysis/Storage.h
new file mode 100644
index 0000000..52899a0
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/Storage.h
@@ -0,0 +1,95 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <vector>
+
+#include <units/time.h>
+
+namespace sysid {
+
+/**
+ * Represents each data point after it is cleaned and various parameters are
+ * calculated.
+ */
+struct PreparedData {
+ /**
+ * The timestamp of the data point.
+ */
+ units::second_t timestamp;
+
+ /**
+ * The voltage of the data point.
+ */
+ double voltage;
+
+ /**
+ * The position of the data point.
+ */
+ double position;
+
+ /**
+ * The velocity of the data point.
+ */
+ double velocity;
+
+ /**
+ * The difference in timestamps between this point and the next point.
+ */
+ units::second_t dt = 0_s;
+
+ /**
+ * The acceleration of the data point
+ */
+ double acceleration = 0.0;
+
+ /**
+ * The cosine value of the data point. This is only used for arm data where we
+ * take the cosine of the position.
+ */
+ double cos = 0.0;
+
+ /**
+ * The sine value of the data point. This is only used for arm data where we
+ * take the sine of the position.
+ */
+ double sin = 0.0;
+
+ /**
+ * Equality operator between PreparedData structs
+ *
+ * @param rhs Another PreparedData struct
+ * @return If the other struct is equal to this one
+ */
+ constexpr bool operator==(const PreparedData& rhs) const {
+ return timestamp == rhs.timestamp && voltage == rhs.voltage &&
+ position == rhs.position && velocity == rhs.velocity &&
+ dt == rhs.dt && acceleration == rhs.acceleration && cos == rhs.cos;
+ }
+};
+
+/**
+ * Storage used by the analysis manger.
+ */
+struct Storage {
+ /**
+ * Dataset for slow (aka quasistatic) test
+ */
+ std::vector<PreparedData> slowForward;
+ std::vector<PreparedData> slowBackward;
+
+ /**
+ * Dataset for fast (aka dynamic) test
+ */
+ std::vector<PreparedData> fastForward;
+ std::vector<PreparedData> fastBackward;
+
+ bool empty() const {
+ return slowForward.empty() || slowBackward.empty() || fastForward.empty() ||
+ fastBackward.empty();
+ }
+};
+
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h b/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h
new file mode 100644
index 0000000..b022072
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <units/angle.h>
+
+namespace sysid {
+/**
+ * Calculates the track width given the left distance, right distance, and
+ * accumulated gyro angle.
+ *
+ * @param l The distance traveled by the left side of the drivetrain.
+ * @param r The distance traveled by the right side of the drivetrain.
+ * @param accum The accumulated gyro angle.
+ */
+double CalculateTrackWidth(double l, double r, units::radian_t accum);
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h b/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
new file mode 100644
index 0000000..85ee09e
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
@@ -0,0 +1,237 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+#include <cstddef>
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/IntegerTopic.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StringTopic.h>
+#include <units/time.h>
+#include <wpi/Logger.h>
+#include <wpi/SmallVector.h>
+#include <wpi/json.h>
+
+#include "sysid/analysis/AnalysisType.h"
+
+namespace sysid {
+/**
+ * This class is responsible for collecting data from the robot and storing it
+ * inside a JSON.
+ */
+class TelemetryManager {
+ public:
+ /**
+ * Represents settings for an instance of the TelemetryManager class. This
+ * contains information about the quasistatic ramp rate for slow tests, the
+ * step voltage for fast tests, and the mechanism type for characterization.
+ */
+ struct Settings {
+ /**
+ * The rate at which the voltage should increase during the quasistatic test
+ * (V/s).
+ */
+ double quasistaticRampRate = 0.25;
+
+ /**
+ * The voltage that the dynamic test should run at (V).
+ */
+ double stepVoltage = 7.0;
+
+ /**
+ * The units the mechanism moves per recorded rotation. The sysid project
+ * will be recording things in rotations of the shaft so the
+ * unitsPerRotation is to convert those measurements to the units the user
+ * wants to use.
+ */
+ double unitsPerRotation = 1.0;
+
+ /**
+ * The name of the units used.
+ * Valid units: "Meters", "Feet", "Inches", "Radians", "Degrees",
+ * "Rotations"
+ */
+ std::string units = "Meters";
+
+ /**
+ * The type of mechanism that will be analyzed.
+ * Supported mechanisms: Drivetrain, Angular Drivetrain, Elevator, Arm,
+ * Simple motor.
+ */
+ AnalysisType mechanism = analysis::kDrivetrain;
+ };
+
+ /**
+ * Constructs an instance of the telemetry manager with the provided settings
+ * and NT instance to collect data over.
+ *
+ * @param settings The settings for this instance of the telemetry manager.
+ * @param logger The logger instance to use for log data.
+ * @param instance The NT instance to collect data over. The default value of
+ * this parameter should suffice in production; it should only
+ * be changed during unit testing.
+ */
+ explicit TelemetryManager(const Settings& settings, wpi::Logger& logger,
+ nt::NetworkTableInstance instance =
+ nt::NetworkTableInstance::GetDefault());
+
+ /**
+ * Begins a test with the given parameters.
+ *
+ * @param name The name of the test.
+ */
+ void BeginTest(std::string_view name);
+
+ /**
+ * Ends the currently running test. If there is no test running, this is a
+ * no-op.
+ */
+ void EndTest();
+
+ /**
+ * Updates the telemetry manager -- this adds a new autospeed entry and
+ * collects newest data from the robot. This must be called periodically by
+ * the user.
+ */
+ void Update();
+
+ /**
+ * Registers a callback that's called by the TelemetryManager when there is a
+ * message to display to the user.
+ *
+ * @param callback Callback function that runs based off of the message
+ */
+ void RegisterDisplayCallback(std::function<void(std::string_view)> callback) {
+ m_callbacks.emplace_back(std::move(callback));
+ }
+
+ /**
+ * Saves a JSON with the stored data at the given location.
+ *
+ * @param location The location to save the JSON at (this is the folder that
+ * should contain the saved JSON).
+ * @return The full file path of the saved JSON.
+ */
+ std::string SaveJSON(std::string_view location);
+
+ /**
+ * Returns whether a test is currently running.
+ *
+ * @return Whether a test is currently running.
+ */
+ bool IsActive() const { return m_isRunningTest; }
+
+ /**
+ * Returns whether the specified test is running or has run.
+ *
+ * @param name The test to check.
+ *
+ * @return Whether the specified test is running or has run.
+ */
+ bool HasRunTest(std::string_view name) const {
+ return std::find(m_tests.cbegin(), m_tests.cend(), name) != m_tests.end();
+ }
+
+ /**
+ * Gets the size of the stored data.
+ *
+ * @return The size of the stored data
+ */
+ size_t GetCurrentDataSize() const { return m_params.data.size(); }
+
+ private:
+ enum class State { WaitingForEnable, RunningTest, WaitingForData };
+
+ /**
+ * Stores information about a currently running test. This information
+ * includes whether the robot will be traveling quickly (dynamic) or slowly
+ * (quasistatic), the direction of movement, the start time of the test,
+ * whether the robot is enabled, the current speed of the robot, and the
+ * collected data.
+ */
+ struct TestParameters {
+ bool fast = false;
+ bool forward = false;
+ bool rotate = false;
+
+ State state = State::WaitingForEnable;
+
+ double enableStart = 0.0;
+ double disableStart = 0.0;
+
+ bool enabled = false;
+ double speed = 0.0;
+
+ std::string raw;
+ std::vector<std::vector<double>> data{};
+ bool overflow = false;
+ bool mechError = false;
+
+ TestParameters() = default;
+ TestParameters(bool fast, bool forward, bool rotate, State state)
+ : fast{fast}, forward{forward}, rotate{rotate}, state{state} {}
+ };
+
+ // Settings for this instance.
+ const Settings& m_settings;
+
+ // Logger.
+ wpi::Logger& m_logger;
+
+ // Test parameters for the currently running test.
+ TestParameters m_params;
+ bool m_isRunningTest = false;
+
+ // A list of running or already run tests.
+ std::vector<std::string> m_tests;
+
+ // Stores the test data.
+ wpi::json m_data;
+
+ // Display callbacks.
+ wpi::SmallVector<std::function<void(std::string_view)>, 1> m_callbacks;
+
+ // NetworkTables instance and entries.
+ nt::NetworkTableInstance m_inst;
+ std::shared_ptr<nt::NetworkTable> table = m_inst.GetTable("SmartDashboard");
+ nt::DoublePublisher m_voltageCommand =
+ table->GetDoubleTopic("SysIdVoltageCommand").Publish();
+ nt::StringPublisher m_testType =
+ table->GetStringTopic("SysIdTestType").Publish();
+ nt::BooleanPublisher m_rotate =
+ table->GetBooleanTopic("SysIdRotate").Publish();
+ nt::StringPublisher m_mechanism =
+ table->GetStringTopic("SysIdTest").Publish();
+ nt::BooleanPublisher m_overflowPub =
+ table->GetBooleanTopic("SysIdOverflow").Publish();
+ nt::BooleanSubscriber m_overflowSub =
+ table->GetBooleanTopic("SysIdOverflow").Subscribe(false);
+ nt::BooleanPublisher m_mechErrorPub =
+ table->GetBooleanTopic("SysIdWrongMech").Publish();
+ nt::BooleanSubscriber m_mechErrorSub =
+ table->GetBooleanTopic("SysIdWrongMech").Subscribe(false);
+ nt::StringSubscriber m_telemetry =
+ table->GetStringTopic("SysIdTelemetry").Subscribe("");
+ nt::IntegerSubscriber m_fmsControlData =
+ m_inst.GetTable("FMSInfo")
+ ->GetIntegerTopic("FMSControlData")
+ .Subscribe(0);
+ nt::DoublePublisher m_ackNumberPub =
+ table->GetDoubleTopic("SysIdAckNumber").Publish();
+ nt::DoubleSubscriber m_ackNumberSub =
+ table->GetDoubleTopic("SysIdAckNumber").Subscribe(0);
+
+ int m_ackNumber;
+};
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/Analyzer.h b/sysid/src/main/native/include/sysid/view/Analyzer.h
new file mode 100644
index 0000000..2f30f61
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/Analyzer.h
@@ -0,0 +1,261 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <thread>
+#include <vector>
+
+#include <glass/View.h>
+#include <implot.h>
+#include <portable-file-dialogs.h>
+#include <units/time.h>
+#include <units/voltage.h>
+#include <wpi/Logger.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/FeedbackAnalysis.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+#include "sysid/view/AnalyzerPlot.h"
+
+struct ImPlotPoint;
+
+namespace glass {
+class Storage;
+} // namespace glass
+
+namespace sysid {
+/**
+ * The Analyzer GUI takes care of providing the user with a user interface to
+ * load their data, visualize the data, adjust certain variables, and then view
+ * the calculated gains.
+ */
+class Analyzer : public glass::View {
+ public:
+ /**
+ * The different display and processing states for the GUI
+ */
+ enum class AnalyzerState {
+ kWaitingForJSON,
+ kNominalDisplay,
+ kMotionThresholdError,
+ kTestDurationError,
+ kGeneralDataError,
+ kFileError
+ };
+ /**
+ * The different motor controller timing presets that can be used.
+ */
+ static constexpr const char* kPresetNames[] = {"Default",
+ "WPILib (2020-)",
+ "WPILib (Pre-2020)",
+ "CANCoder",
+ "CTRE (Pro)",
+ "CTRE",
+ "REV Brushless Encoder Port",
+ "REV Brushed Encoder Port",
+ "REV Data Port",
+ "Venom"};
+
+ /**
+ * The different control loops that can be used.
+ */
+ static constexpr const char* kLoopTypes[] = {"Position", "Velocity"};
+
+ /**
+ * Linear drivetrain analysis subsets
+ */
+ static constexpr const char* kDatasets[] = {"Combined", "Left", "Right"};
+
+ /**
+ * Creates the Analyzer widget
+ *
+ * @param storage Glass Storage
+ * @param logger The program logger
+ */
+ Analyzer(glass::Storage& storage, wpi::Logger& logger);
+
+ /**
+ * Displays the analyzer widget
+ */
+ void Display() override;
+
+ ~Analyzer() override { AbortDataPrep(); };
+
+ private:
+ /**
+ * Handles the logic for selecting a json to analyze
+ */
+ void SelectFile();
+
+ /**
+ * Kills the data preparation thread
+ */
+ void AbortDataPrep();
+
+ /**
+ * Displays the settings to adjust trimming and filtering for feedforward
+ * gains.
+ */
+ void DisplayFeedforwardParameters(float beginX, float beginY);
+
+ /**
+ * Displays the graphs of the data.
+ */
+ void DisplayGraphs();
+
+ /**
+ * Displays the file selection widget.
+ */
+ void DisplayFileSelector();
+
+ /**
+ * Resets the current analysis data.
+ */
+ void ResetData();
+
+ /**
+ * Sets up the reset button and Unit override buttons.
+ *
+ * @return True if the tool had been reset.
+ */
+ bool DisplayResetAndUnitOverride();
+
+ /**
+ * Prepares the data for analysis.
+ */
+ void PrepareData();
+
+ /**
+ * Sets up the graphs to display Raw Data.
+ */
+ void PrepareRawGraphs();
+
+ /**
+ * Sets up the graphs to display filtered/processed data.
+ */
+ void PrepareGraphs();
+
+ /**
+ * True if the stored state is associated with an error.
+ */
+ bool IsErrorState();
+
+ /**
+ * True if the stored state is associated with a data processing error.
+ */
+ bool IsDataErrorState();
+
+ /**
+ * Displays inputs to allow the collecting of theoretical feedforward gains.
+ */
+ void CollectFeedforwardGains(float beginX, float beginY);
+
+ /**
+ * Displays calculated feedforward gains.
+ */
+ void DisplayFeedforwardGains(float beginX, float beginY);
+
+ /**
+ * Displays calculated feedback gains.
+ */
+ void DisplayFeedbackGains();
+
+ /**
+ * Estimates ideal step test duration, qp, and qv for the LQR based off of the
+ * data given
+ */
+ void ConfigParamsOnFileSelect();
+
+ /**
+ * Updates feedforward gains from the analysis manager.
+ */
+ void UpdateFeedforwardGains();
+
+ /**
+ * Updates feedback gains from the analysis manager.
+ */
+ void UpdateFeedbackGains();
+
+ /**
+ * Handles logic of displaying a gain on ImGui
+ */
+ bool DisplayGain(const char* text, double* data, bool readOnly);
+
+ /**
+ * Handles errors when they pop up.
+ */
+ void HandleError(std::string_view msg);
+
+ // State of the Display GUI
+ AnalyzerState m_state = AnalyzerState::kWaitingForJSON;
+
+ // Stores the exception message.
+ std::string m_exception;
+
+ bool m_calcDefaults = false;
+
+ // This is true if the error popup needs to be displayed
+ bool m_errorPopup = false;
+
+ // Everything related to feedback controller calculations.
+ AnalysisManager::Settings m_settings;
+ wpi::StringMap<FeedbackControllerPreset> m_presets;
+
+ int m_selectedLoopType = 1;
+ int m_selectedPreset = 0;
+
+ // Feedforward and feedback gains.
+ std::vector<double> m_ff;
+ double m_accelRSquared;
+ double m_accelRMSE;
+ double m_Kp;
+ double m_Kd;
+ units::millisecond_t m_timescale;
+
+ // Track width
+ std::optional<double> m_trackWidth;
+
+ // Units
+ int m_selectedOverrideUnit = 0;
+ double m_conversionFactor = 0.0;
+
+ // Data analysis
+ std::unique_ptr<AnalysisManager> m_manager;
+ int m_dataset = 0;
+ int m_window = 8;
+ double m_threshold = 0.2;
+ float m_stepTestDuration = 0.0;
+
+ double m_gearingNumerator = 1.0;
+ double m_gearingDenominator = 1.0;
+
+ bool combinedGraphFit = false;
+
+ // File manipulation
+ std::unique_ptr<pfd::open_file> m_selector;
+ std::string m_location;
+
+ // Logger
+ wpi::Logger& m_logger;
+
+ // Plot
+ AnalyzerPlot m_plot{m_logger};
+ bool m_prevPlotsLoaded = false;
+
+ // Stores graph scroll bar position and states for keeping track of scroll
+ // positions after loading graphs
+ float m_graphScroll = 0;
+
+ std::atomic<bool> m_abortDataPrep{false};
+ std::thread m_dataThread;
+};
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h b/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h
new file mode 100644
index 0000000..e5fbe8c
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h
@@ -0,0 +1,203 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+#include <atomic>
+#include <functional>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <imgui.h>
+#include <implot.h>
+#include <units/time.h>
+#include <wpi/Logger.h>
+#include <wpi/spinlock.h>
+
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/Storage.h"
+
+namespace sysid {
+/**
+ * Class that helps with plotting data in the analyzer view.
+ */
+class AnalyzerPlot {
+ public:
+ float m_pointSize = 1.25;
+ // 0 for forward, 1 for reverse
+ int m_direction = 0;
+
+ /**
+ * Constructs an instance of the analyzer plot helper and allocates memory for
+ * all data vectors.
+ *
+ * @param logger The program logger
+ */
+ explicit AnalyzerPlot(wpi::Logger& logger);
+
+ /**
+ * Sets the data to be displayed on the plots.
+ *
+ * @param rawData Raw data storage.
+ * @param filteredData Filtered data storage.
+ * @param unit Unit of the dataset
+ * @param ff List of feedforward gains (Ks, Kv, Ka, and optionally
+ * Kg).
+ * @param startTimes Array of dataset start times.
+ * @param type Type of analysis.
+ * @param abort Aborts analysis early if set to true from another
+ * thread.
+ */
+ void SetData(const Storage& rawData, const Storage& filteredData,
+ std::string_view unit, const std::vector<double>& ff,
+ const std::array<units::second_t, 4>& startTimes,
+ AnalysisType type, std::atomic<bool>& abort);
+
+ /**
+ * Utility method to plot the raw time series data
+ *
+ * @param rawSlow The raw slow (quasistatic) test data
+ * @param rawFast The raw fast (dynamic) test data
+ * @param abort Aborts analysis early if set to true from another thread
+ */
+ void SetRawTimeData(const std::vector<PreparedData>& rawSlow,
+ const std::vector<PreparedData>& rawFast,
+ std::atomic<bool>& abort);
+
+ /**
+ * Utility method to reset everything before generating the points to plot.
+ */
+ void ResetData();
+
+ /**
+ * Utility method to get set the graph labels based off of the units
+ *
+ * @param unit Unit of the dataset
+ */
+ void SetGraphLabels(std::string_view unit);
+
+ /**
+ * Sets up only the raw time series data to be plotted. This is mainly
+ * intended to be used if the filtered data has issues with it.
+ *
+ * @param data The raw data.
+ * @param unit Unit of the dataset.
+ * @param abort Aborts analysis early if set to true from another thread.
+ */
+ void SetRawData(const Storage& data, std::string_view unit,
+ std::atomic<bool>& abort);
+
+ /**
+ * Displays time domain plots.
+ *
+ * @return Returns true if plots aren't in the loading state
+ */
+ bool DisplayPlots();
+
+ /**
+ * Sets certain flags to true so that the GUI automatically fits the plots
+ */
+ void FitPlots();
+
+ /**
+ * Gets the pointer to the stored Root Mean Squared Error for display
+ *
+ * @return A pointer to the RMSE
+ */
+ double* GetSimRMSE();
+
+ /**
+ * Gets the pointer to the stored simulated velocity R-squared for display
+ *
+ * @return A pointer to the R-squared
+ */
+ double* GetSimRSquared();
+
+ private:
+ // The maximum size of each vector (dataset to plot)
+ static constexpr size_t kMaxSize = 2048;
+
+ struct FilteredDataVsTimePlot {
+ std::vector<ImPlotPoint> rawData;
+ std::vector<ImPlotPoint> filteredData;
+
+ // Simulated time domain data
+ std::vector<std::vector<ImPlotPoint>> simData;
+
+ // Stores whether this was the first call to Plot() since setting data
+ bool fitNextPlot = false;
+
+ FilteredDataVsTimePlot();
+
+ /**
+ * Plots data and fit line.
+ *
+ * @param title Plot title.
+ * @param size Plot size.
+ * @param yLabel Y axis label.
+ * @param pointSize The size of the data point markers (in pixels).
+ */
+ void Plot(const char* title, const ImVec2& size, const char* yLabel,
+ float pointSize);
+
+ /**
+ * Clears plot.
+ */
+ void Clear();
+ };
+
+ struct DataWithFitLinePlot {
+ std::vector<ImPlotPoint> data;
+ std::array<ImPlotPoint, 2> fitLine;
+
+ // Stores whether this was the first call to Plot() since setting data
+ bool fitNextPlot = false;
+
+ DataWithFitLinePlot();
+
+ /**
+ * Plots data and fit line.
+ *
+ * @param title Plot title.
+ * @param size Plot size.
+ * @param xLabel X axis label.
+ * @param yLabel Y axis label.
+ * @param fitX True if X axis should be autofitted.
+ * @param fitY True if Y axis should be autofitted.
+ * @param pointSize The size of the data point markers (in pixels).
+ * @param setup Callback within BeginPlot() block that performs custom plot
+ * setup.
+ */
+ void Plot(
+ const char* title, const ImVec2& size, const char* xLabel,
+ const char* yLabel, bool fitX, bool fitY, float pointSize,
+ std::function<void()> setup = [] {});
+
+ /**
+ * Clears plot.
+ */
+ void Clear();
+ };
+
+ std::string m_velocityLabel;
+ std::string m_accelerationLabel;
+ std::string m_velPortionAccelLabel;
+
+ // Thread safety
+ wpi::spinlock m_mutex;
+
+ // Logger
+ wpi::Logger& m_logger;
+
+ FilteredDataVsTimePlot m_quasistaticData;
+ FilteredDataVsTimePlot m_dynamicData;
+ DataWithFitLinePlot m_regressionData;
+ DataWithFitLinePlot m_timestepData;
+
+ double m_RMSE;
+ double m_accelRSquared;
+};
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/JSONConverter.h b/sysid/src/main/native/include/sysid/view/JSONConverter.h
new file mode 100644
index 0000000..89bfa32
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/JSONConverter.h
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+
+#include <glass/View.h>
+#include <portable-file-dialogs.h>
+#include <wpi/Logger.h>
+
+namespace sysid {
+/**
+ * Helps with converting different JSONs into different formats. Primarily
+ * enables users to convert an old 2020 FRC-Characterization JSON into a SysId
+ * JSON or a SysId JSON into a CSV file.
+ */
+class JSONConverter {
+ public:
+ /**
+ * Creates a JSONConverter widget
+ *
+ * @param logger The program logger
+ */
+ explicit JSONConverter(wpi::Logger& logger) : m_logger(logger) {}
+
+ /**
+ * Function to display the SysId JSON to CSV converter.
+ */
+ void DisplayCSVConvert();
+
+ private:
+ /**
+ * Helper method to display a specific JSON converter
+ *
+ * @param tooltip The tooltip describing the JSON converter
+ * @param converter The function that takes a filename path and performs the
+ * previously specifid JSON conversion.
+ */
+ void DisplayConverter(
+ const char* tooltip,
+ std::function<std::string(std::string_view, wpi::Logger&)> converter);
+
+ wpi::Logger& m_logger;
+
+ std::string m_location;
+ std::unique_ptr<pfd::open_file> m_opener;
+
+ std::string m_exception;
+
+ double m_timestamp = 0;
+};
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/Logger.h b/sysid/src/main/native/include/sysid/view/Logger.h
new file mode 100644
index 0000000..d06d650
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/Logger.h
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <glass/DataSource.h>
+#include <glass/View.h>
+#include <glass/networktables/NetworkTablesSettings.h>
+#include <portable-file-dialogs.h>
+#include <wpi/Logger.h>
+
+#include "sysid/telemetry/TelemetryManager.h"
+
+namespace glass {
+class Storage;
+} // namespace glass
+
+namespace sysid {
+/**
+ * The logger GUI takes care of running the system idenfitication tests over
+ * NetworkTables and logging the data. This data is then stored in a JSON file
+ * which can be used for analysis.
+ */
+class Logger : public glass::View {
+ public:
+ /**
+ * Makes a logger widget.
+ *
+ * @param storage The glass storage object
+ * @param logger A logger object that keeps track of the program's logs
+ */
+ Logger(glass::Storage& storage, wpi::Logger& logger);
+
+ /**
+ * Displays the logger widget.
+ */
+ void Display() override;
+
+ /**
+ * The different mechanism / analysis types that are supported.
+ */
+ static constexpr const char* kTypes[] = {"Drivetrain", "Drivetrain (Angular)",
+ "Arm", "Elevator", "Simple"};
+
+ /**
+ * The different units that are supported.
+ */
+ static constexpr const char* kUnits[] = {"Meters", "Feet", "Inches",
+ "Radians", "Rotations", "Degrees"};
+
+ private:
+ /**
+ * Handles the logic of selecting a folder to save the SysId JSON to
+ */
+ void SelectDataFolder();
+
+ wpi::Logger& m_logger;
+
+ TelemetryManager::Settings m_settings;
+ int m_selectedType = 0;
+ int m_selectedUnit = 0;
+
+ std::unique_ptr<TelemetryManager> m_manager =
+ std::make_unique<TelemetryManager>(m_settings, m_logger);
+
+ std::unique_ptr<pfd::select_folder> m_selector;
+ std::string m_jsonLocation;
+
+ glass::NetworkTablesSettings m_ntSettings;
+
+ bool m_isRotationalUnits = false;
+
+ std::string m_popupText;
+
+ std::string m_opened;
+ std::string m_exception;
+};
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/UILayout.h b/sysid/src/main/native/include/sysid/view/UILayout.h
new file mode 100644
index 0000000..732a1aa
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/UILayout.h
@@ -0,0 +1,95 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <imgui.h>
+
+namespace sysid {
+/**
+ * constexpr shim for ImVec2.
+ */
+struct Vector2d {
+ /**
+ * X coordinate.
+ */
+ float x = 0;
+
+ /**
+ * Y coordinate.
+ */
+ float y = 0;
+
+ /**
+ * Vector2d addition operator.
+ *
+ * @param rhs Vector to add.
+ * @return Sum of two vectors.
+ */
+ constexpr Vector2d operator+(const Vector2d& rhs) const {
+ return Vector2d{x + rhs.x, y + rhs.y};
+ }
+
+ /**
+ * Vector2d subtraction operator.
+ *
+ * @param rhs Vector to subtract.
+ * @return Difference of two vectors.
+ */
+ constexpr Vector2d operator-(const Vector2d& rhs) const {
+ return Vector2d{x - rhs.x, y - rhs.y};
+ }
+
+ /**
+ * Conversion operator to ImVec2.
+ */
+ explicit operator ImVec2() const { return ImVec2{x, y}; }
+};
+
+// App window size
+inline constexpr Vector2d kAppWindowSize{1280, 720};
+
+// Menubar height
+inline constexpr int kMenubarHeight = 20;
+
+// Gap between window edges
+inline constexpr int kWindowGap = 5;
+
+// Left column position and size
+inline constexpr Vector2d kLeftColPos{kWindowGap, kMenubarHeight + kWindowGap};
+inline constexpr Vector2d kLeftColSize{
+ 310, kAppWindowSize.y - kLeftColPos.y - kWindowGap};
+
+// Left column contents
+inline constexpr Vector2d kLoggerWindowPos = kLeftColPos;
+inline constexpr Vector2d kLoggerWindowSize{
+ kLeftColSize.x, kAppWindowSize.y - kWindowGap - kLoggerWindowPos.y};
+
+// Center column position and size
+inline constexpr Vector2d kCenterColPos =
+ kLeftColPos + Vector2d{kLeftColSize.x + kWindowGap, 0};
+inline constexpr Vector2d kCenterColSize{
+ 360, kAppWindowSize.y - kLeftColPos.y - kWindowGap};
+
+// Center column contents
+inline constexpr Vector2d kAnalyzerWindowPos = kCenterColPos;
+inline constexpr Vector2d kAnalyzerWindowSize{kCenterColSize.x, 550};
+inline constexpr Vector2d kProgramLogWindowPos =
+ kAnalyzerWindowPos + Vector2d{0, kAnalyzerWindowSize.y + kWindowGap};
+inline constexpr Vector2d kProgramLogWindowSize{
+ kCenterColSize.x, kAppWindowSize.y - kWindowGap - kProgramLogWindowPos.y};
+
+// Right column position and size
+inline constexpr Vector2d kRightColPos =
+ kCenterColPos + Vector2d{kCenterColSize.x + kWindowGap, 0};
+inline constexpr Vector2d kRightColSize =
+ kAppWindowSize - kRightColPos - Vector2d{kWindowGap, kWindowGap};
+
+// Right column contents
+inline constexpr Vector2d kDiagnosticPlotWindowPos = kRightColPos;
+inline constexpr Vector2d kDiagnosticPlotWindowSize = kRightColSize;
+
+// Text box width as a multiple of the font size
+inline constexpr int kTextBoxWidthMultiple = 10;
+} // namespace sysid
diff --git a/sysid/src/main/native/mac/sysid.icns b/sysid/src/main/native/mac/sysid.icns
new file mode 100644
index 0000000..53f0d69
--- /dev/null
+++ b/sysid/src/main/native/mac/sysid.icns
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-128.png b/sysid/src/main/native/resources/sysid-128.png
new file mode 100644
index 0000000..1da8dfe
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-128.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-16.png b/sysid/src/main/native/resources/sysid-16.png
new file mode 100644
index 0000000..851739f
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-16.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-256.png b/sysid/src/main/native/resources/sysid-256.png
new file mode 100644
index 0000000..a66aa94
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-256.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-32.png b/sysid/src/main/native/resources/sysid-32.png
new file mode 100644
index 0000000..3332bd1
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-32.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-48.png b/sysid/src/main/native/resources/sysid-48.png
new file mode 100644
index 0000000..bee41b7
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-48.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-512.png b/sysid/src/main/native/resources/sysid-512.png
new file mode 100644
index 0000000..5fc1d8e
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-512.png
Binary files differ
diff --git a/sysid/src/main/native/resources/sysid-64.png b/sysid/src/main/native/resources/sysid-64.png
new file mode 100644
index 0000000..efb193d
--- /dev/null
+++ b/sysid/src/main/native/resources/sysid-64.png
Binary files differ
diff --git a/sysid/src/main/native/win/sysid.ico b/sysid/src/main/native/win/sysid.ico
new file mode 100644
index 0000000..89f6d3f
--- /dev/null
+++ b/sysid/src/main/native/win/sysid.ico
Binary files differ
diff --git a/sysid/src/main/native/win/sysid.rc b/sysid/src/main/native/win/sysid.rc
new file mode 100644
index 0000000..21d11e8
--- /dev/null
+++ b/sysid/src/main/native/win/sysid.rc
@@ -0,0 +1 @@
+IDI_ICON1 ICON "sysid.ico"
diff --git a/sysid/src/test/native/cpp/Main.cpp b/sysid/src/test/native/cpp/Main.cpp
new file mode 100644
index 0000000..e993c1f
--- /dev/null
+++ b/sysid/src/test/native/cpp/Main.cpp
@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+int main(int argc, char** argv) {
+ ::testing::InitGoogleTest(&argc, argv);
+ int ret = RUN_ALL_TESTS();
+ return ret;
+}
diff --git a/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp b/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
new file mode 100644
index 0000000..0abb2a1
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/AnalysisType.h"
+
+TEST(AnalysisTypeTest, FromName) {
+ EXPECT_EQ(sysid::analysis::kDrivetrain,
+ sysid::analysis::FromName("Drivetrain"));
+ EXPECT_EQ(sysid::analysis::kDrivetrainAngular,
+ sysid::analysis::FromName("Drivetrain (Angular)"));
+ EXPECT_EQ(sysid::analysis::kElevator, sysid::analysis::FromName("Elevator"));
+ EXPECT_EQ(sysid::analysis::kArm, sysid::analysis::FromName("Arm"));
+ EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Simple"));
+ EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Random"));
+}
diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
new file mode 100644
index 0000000..44f664c
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/FeedbackAnalysis.h"
+#include "sysid/analysis/FeedbackControllerPreset.h"
+
+TEST(FeedbackAnalysisTest, Velocity1) {
+ auto Kv = 3.060;
+ auto Ka = 0.327;
+
+ sysid::LQRParameters params{1, 1.5, 7};
+
+ auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+ sysid::presets::kDefault, params, Kv, Ka);
+
+ EXPECT_NEAR(Kp, 2.11, 0.05);
+ EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, Velocity2) {
+ auto Kv = 0.0693;
+ auto Ka = 0.1170;
+
+ sysid::LQRParameters params{1, 1.5, 7};
+
+ auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+ sysid::presets::kDefault, params, Kv, Ka);
+
+ EXPECT_NEAR(Kp, 3.11, 0.05);
+ EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityConversion) {
+ auto Kv = 0.0693;
+ auto Ka = 0.1170;
+
+ sysid::LQRParameters params{1, 1.5, 7};
+
+ auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+ sysid::presets::kDefault, params, Kv, Ka, 3.0 * 1023);
+
+ // This should have the same Kp as the test above, but scaled by a factor of 3
+ // * 1023.
+ EXPECT_NEAR(Kp, 3.11 / (3 * 1023), 0.005);
+ EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityCTRE) {
+ auto Kv = 1.97;
+ auto Ka = 0.179;
+
+ sysid::LQRParameters params{1, 1.5, 7};
+
+ auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+ sysid::presets::kCTRECANCoder, params, Kv, Ka);
+
+ EXPECT_NEAR(Kp, 0.000417, 0.00005);
+ EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityCTREConversion) {
+ auto Kv = 1.97;
+ auto Ka = 0.179;
+
+ sysid::LQRParameters params{1, 1.5, 7};
+
+ auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+ sysid::presets::kCTRECANCoder, params, Kv, Ka, 3.0);
+
+ // This should have the same Kp as the test above, but scaled by a factor
+ // of 3.
+ EXPECT_NEAR(Kp, 0.000417 / 3, 0.00005);
+ EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityREV) {
+ auto Kv = 1.97;
+ auto Ka = 0.179;
+
+ sysid::LQRParameters params{1, 1.5, 7};
+
+ auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+ sysid::presets::kREVNEOBuiltIn, params, Kv, Ka);
+
+ EXPECT_NEAR(Kp, 0.00241, 0.005);
+ EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, VelocityREVConversion) {
+ auto Kv = 1.97;
+ auto Ka = 0.179;
+
+ sysid::LQRParameters params{1, 1.5, 7};
+
+ auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+ sysid::presets::kREVNEOBuiltIn, params, Kv, Ka, 3.0);
+
+ // This should have the same Kp as the test above, but scaled by a factor
+ // of 3.
+ EXPECT_NEAR(Kp, 0.00241 / 3, 0.005);
+ EXPECT_NEAR(Kd, 0.00, 0.05);
+}
diff --git a/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
new file mode 100644
index 0000000..a52840d
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
@@ -0,0 +1,251 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/ArmSim.h"
+#include "sysid/analysis/ElevatorSim.h"
+#include "sysid/analysis/FeedforwardAnalysis.h"
+#include "sysid/analysis/SimpleMotorSim.h"
+
+/**
+ * Return simulated test data for a given simulation model.
+ *
+ * @param Ks Static friction gain.
+ * @param Kv Velocity gain.
+ * @param Ka Acceleration gain.
+ * @param Kg Gravity cosine gain.
+ */
+template <typename Model>
+sysid::Storage CollectData(Model& model) {
+ constexpr auto kUstep = 0.25_V / 1_s;
+ constexpr units::volt_t kUmax = 7_V;
+ constexpr units::second_t T = 5_ms;
+ constexpr units::second_t kTestDuration = 5_s;
+
+ sysid::Storage storage;
+ auto& [slowForward, slowBackward, fastForward, fastBackward] = storage;
+
+ // Slow forward test
+ auto voltage = 0_V;
+ for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+ slowForward.emplace_back(sysid::PreparedData{
+ i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+ model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+ std::sin(model.GetPosition())});
+
+ model.Update(voltage, T);
+ voltage += kUstep * T;
+ }
+
+ // Slow backward test
+ model.Reset();
+ voltage = 0_V;
+ for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+ slowBackward.emplace_back(sysid::PreparedData{
+ i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+ model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+ std::sin(model.GetPosition())});
+
+ model.Update(voltage, T);
+ voltage -= kUstep * T;
+ }
+
+ // Fast forward test
+ model.Reset();
+ voltage = 0_V;
+ for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+ fastForward.emplace_back(sysid::PreparedData{
+ i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+ model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+ std::sin(model.GetPosition())});
+
+ model.Update(voltage, T);
+ voltage = kUmax;
+ }
+
+ // Fast backward test
+ model.Reset();
+ voltage = 0_V;
+ for (int i = 0; i < (kTestDuration / T).value(); ++i) {
+ fastBackward.emplace_back(sysid::PreparedData{
+ i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T,
+ model.GetAcceleration(voltage), std::cos(model.GetPosition()),
+ std::sin(model.GetPosition())});
+
+ model.Update(voltage, T);
+ voltage = -kUmax;
+ }
+
+ return storage;
+}
+
+TEST(FeedforwardAnalysisTest, Arm1) {
+ constexpr double Ks = 1.01;
+ constexpr double Kv = 3.060;
+ constexpr double Ka = 0.327;
+ constexpr double Kg = 0.211;
+
+ for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
+ sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+ sysid::analysis::kArm);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+ EXPECT_NEAR(gains[3], Kg, 0.003);
+ EXPECT_NEAR(gains[4], offset, 0.007);
+ }
+}
+
+TEST(FeedforwardAnalysisTest, Arm2) {
+ constexpr double Ks = 0.547;
+ constexpr double Kv = 0.0693;
+ constexpr double Ka = 0.1170;
+ constexpr double Kg = 0.122;
+
+ for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
+ sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+ sysid::analysis::kArm);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+ EXPECT_NEAR(gains[3], Kg, 0.003);
+ EXPECT_NEAR(gains[4], offset, 0.007);
+ }
+}
+
+TEST(FeedforwardAnalysisTest, Drivetrain1) {
+ constexpr double Ks = 1.01;
+ constexpr double Kv = 3.060;
+ constexpr double Ka = 0.327;
+
+ sysid::SimpleMotorSim model{Ks, Kv, Ka};
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+ sysid::analysis::kDrivetrain);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Drivetrain2) {
+ constexpr double Ks = 0.547;
+ constexpr double Kv = 0.0693;
+ constexpr double Ka = 0.1170;
+
+ sysid::SimpleMotorSim model{Ks, Kv, Ka};
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+ sysid::analysis::kDrivetrain);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, DrivetrainAngular1) {
+ constexpr double Ks = 1.01;
+ constexpr double Kv = 3.060;
+ constexpr double Ka = 0.327;
+
+ sysid::SimpleMotorSim model{Ks, Kv, Ka};
+ auto ff = sysid::CalculateFeedforwardGains(
+ CollectData(model), sysid::analysis::kDrivetrainAngular);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, DrivetrainAngular2) {
+ constexpr double Ks = 0.547;
+ constexpr double Kv = 0.0693;
+ constexpr double Ka = 0.1170;
+
+ sysid::SimpleMotorSim model{Ks, Kv, Ka};
+ auto ff = sysid::CalculateFeedforwardGains(
+ CollectData(model), sysid::analysis::kDrivetrainAngular);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Elevator1) {
+ constexpr double Ks = 1.01;
+ constexpr double Kv = 3.060;
+ constexpr double Ka = 0.327;
+ constexpr double Kg = -0.211;
+
+ sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+ sysid::analysis::kElevator);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+ EXPECT_NEAR(gains[3], Kg, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Elevator2) {
+ constexpr double Ks = 0.547;
+ constexpr double Kv = 0.0693;
+ constexpr double Ka = 0.1170;
+ constexpr double Kg = -0.122;
+
+ sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+ sysid::analysis::kElevator);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+ EXPECT_NEAR(gains[3], Kg, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Simple1) {
+ constexpr double Ks = 1.01;
+ constexpr double Kv = 3.060;
+ constexpr double Ka = 0.327;
+
+ sysid::SimpleMotorSim model{Ks, Kv, Ka};
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+ sysid::analysis::kSimple);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+}
+
+TEST(FeedforwardAnalysisTest, Simple2) {
+ constexpr double Ks = 0.547;
+ constexpr double Kv = 0.0693;
+ constexpr double Ka = 0.1170;
+
+ sysid::SimpleMotorSim model{Ks, Kv, Ka};
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
+ sysid::analysis::kSimple);
+ auto& gains = std::get<0>(ff);
+
+ EXPECT_NEAR(gains[0], Ks, 0.003);
+ EXPECT_NEAR(gains[1], Kv, 0.003);
+ EXPECT_NEAR(gains[2], Ka, 0.003);
+}
diff --git a/sysid/src/test/native/cpp/analysis/FilterTest.cpp b/sysid/src/test/native/cpp/analysis/FilterTest.cpp
new file mode 100644
index 0000000..a7b0349
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/FilterTest.cpp
@@ -0,0 +1,170 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <array>
+#include <cmath>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/FeedforwardAnalysis.h"
+#include "sysid/analysis/FilteringUtils.h"
+#include "sysid/analysis/Storage.h"
+
+TEST(FilterTest, MedianFilter) {
+ std::vector<sysid::PreparedData> testData{
+ sysid::PreparedData{0_s, 0, 0, 0}, sysid::PreparedData{0_s, 0, 0, 1},
+ sysid::PreparedData{0_s, 0, 0, 10}, sysid::PreparedData{0_s, 0, 0, 5},
+ sysid::PreparedData{0_s, 0, 0, 3}, sysid::PreparedData{0_s, 0, 0, 0},
+ sysid::PreparedData{0_s, 0, 0, 1000}, sysid::PreparedData{0_s, 0, 0, 7},
+ sysid::PreparedData{0_s, 0, 0, 6}, sysid::PreparedData{0_s, 0, 0, 5}};
+ std::vector<sysid::PreparedData> expectedData{
+ sysid::PreparedData{0_s, 0, 0, 0}, sysid::PreparedData{0_s, 0, 0, 1},
+ sysid::PreparedData{0_s, 0, 0, 5}, sysid::PreparedData{0_s, 0, 0, 5},
+ sysid::PreparedData{0_s, 0, 0, 3}, sysid::PreparedData{0_s, 0, 0, 3},
+ sysid::PreparedData{0_s, 0, 0, 7}, sysid::PreparedData{0_s, 0, 0, 7},
+ sysid::PreparedData{0_s, 0, 0, 6}, sysid::PreparedData{0_s, 0, 0, 5}};
+
+ sysid::ApplyMedianFilter(&testData, 3);
+ EXPECT_EQ(expectedData, testData);
+}
+
+TEST(FilterTest, NoiseFloor) {
+ std::vector<sysid::PreparedData> testData = {
+ {0_s, 1, 2, 3, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 1, 0},
+ {2_s, 1, 2, 3, 5_ms, 2, 0}, {3_s, 1, 2, 3, 5_ms, 5, 0},
+ {4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
+ {6_s, 1, 2, 3, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0},
+ {8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0}};
+ double noiseFloor =
+ GetNoiseFloor(testData, 2, [](auto&& pt) { return pt.acceleration; });
+ EXPECT_NEAR(0.953, noiseFloor, 0.001);
+}
+
+TEST(FilterTest, StepTrim) {
+ std::vector<sysid::PreparedData> testData = {
+ {0_s, 1, 2, 3, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 0.25, 0},
+ {2_s, 1, 2, 3, 5_ms, 0.5, 0}, {3_s, 1, 2, 3, 5_ms, 0.45, 0},
+ {4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
+ {6_s, 1, 2, 3, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0},
+ {8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0},
+ };
+
+ std::vector<sysid::PreparedData> expectedData = {
+ {2_s, 1, 2, 3, 5_ms, 0.5, 0},
+ {3_s, 1, 2, 3, 5_ms, 0.45, 0},
+ {4_s, 1, 2, 3, 5_ms, 0.35, 0},
+ {5_s, 1, 2, 3, 5_ms, 0.15, 0}};
+
+ auto maxTime = 9_s;
+ auto minTime = maxTime;
+
+ sysid::AnalysisManager::Settings settings;
+ auto [tempMinTime, positionDelay, velocityDelay] =
+ sysid::TrimStepVoltageData(&testData, &settings, minTime, maxTime);
+ minTime = tempMinTime;
+
+ EXPECT_EQ(expectedData[0].acceleration, testData[0].acceleration);
+ EXPECT_EQ(expectedData.back().acceleration, testData.back().acceleration);
+ EXPECT_EQ(5, settings.stepTestDuration.value());
+ EXPECT_EQ(2, minTime.value());
+}
+
+template <int Derivative, int Samples, typename F, typename DfDx>
+void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
+ double max) {
+ static_assert(Samples % 2 != 0, "Number of samples must be odd.");
+
+ auto filter = sysid::CentralFiniteDifference<Derivative, Samples>(h);
+
+ for (int i = min / h.value(); i < max / h.value(); ++i) {
+ // Let filter initialize
+ if (i < static_cast<int>(min / h.value()) + Samples) {
+ filter.Calculate(f(i * h.value()));
+ continue;
+ }
+
+ // For central finite difference, the derivative computed at this point is
+ // half the window size in the past.
+ // The order of accuracy is O(h^(N - d)) where N is number of stencil
+ // points and d is order of derivative
+ EXPECT_NEAR(dfdx((i - static_cast<int>((Samples - 1) / 2)) * h.value()),
+ filter.Calculate(f(i * h.value())),
+ std::pow(h.value(), Samples - Derivative));
+ }
+}
+
+/**
+ * Test central finite difference.
+ */
+TEST(LinearFilterOutputTest, CentralFiniteDifference) {
+ constexpr auto h = 5_ms;
+
+ AssertCentralResults<1, 3>(
+ [](double x) {
+ // f(x) = x²
+ return x * x;
+ },
+ [](double x) {
+ // df/dx = 2x
+ return 2.0 * x;
+ },
+ h, -20.0, 20.0);
+
+ AssertCentralResults<1, 3>(
+ [](double x) {
+ // f(x) = std::sin(x)
+ return std::sin(x);
+ },
+ [](double x) {
+ // df/dx = std::cos(x)
+ return std::cos(x);
+ },
+ h, -20.0, 20.0);
+
+ AssertCentralResults<1, 3>(
+ [](double x) {
+ // f(x) = ln(x)
+ return std::log(x);
+ },
+ [](double x) {
+ // df/dx = 1 / x
+ return 1.0 / x;
+ },
+ h, 1.0, 20.0);
+
+ AssertCentralResults<2, 5>(
+ [](double x) {
+ // f(x) = x^2
+ return x * x;
+ },
+ [](double x) {
+ // d²f/dx² = 2
+ return 2.0;
+ },
+ h, -20.0, 20.0);
+
+ AssertCentralResults<2, 5>(
+ [](double x) {
+ // f(x) = std::sin(x)
+ return std::sin(x);
+ },
+ [](double x) {
+ // d²f/dx² = -std::sin(x)
+ return -std::sin(x);
+ },
+ h, -20.0, 20.0);
+
+ AssertCentralResults<2, 5>(
+ [](double x) {
+ // f(x) = ln(x)
+ return std::log(x);
+ },
+ [](double x) {
+ // d²f/dx² = -1 / x²
+ return -1.0 / (x * x);
+ },
+ h, 1.0, 20.0);
+}
diff --git a/sysid/src/test/native/cpp/analysis/OLSTest.cpp b/sysid/src/test/native/cpp/analysis/OLSTest.cpp
new file mode 100644
index 0000000..bf20516
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/OLSTest.cpp
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/OLS.h"
+
+TEST(OLSTest, TwoVariablesTwoPoints) {
+ // (1, 3) and (2, 5). Should produce y = 2x + 1.
+ Eigen::MatrixXd X{{1.0, 1.0}, {1.0, 2.0}};
+ Eigen::VectorXd y{{3.0}, {5.0}};
+
+ auto [coefficients, cod, rmse] = sysid::OLS(X, y);
+ EXPECT_EQ(coefficients.size(), 2u);
+
+ EXPECT_NEAR(coefficients[0], 1.0, 0.05);
+ EXPECT_NEAR(coefficients[1], 2.0, 0.05);
+ EXPECT_NEAR(cod, 1.0, 1E-4);
+}
+
+TEST(OLSTest, TwoVariablesFivePoints) {
+ // (2, 4), (3, 5), (5, 7), (7, 10), (9, 15)
+ // Should produce 1.518x + 0.305.
+ Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 5}, {1, 7}, {1, 9}};
+ Eigen::VectorXd y{{4}, {5}, {7}, {10}, {15}};
+
+ auto [coefficients, cod, rmse] = sysid::OLS(X, y);
+ EXPECT_EQ(coefficients.size(), 2u);
+
+ EXPECT_NEAR(coefficients[0], 0.305, 0.05);
+ EXPECT_NEAR(coefficients[1], 1.518, 0.05);
+ EXPECT_NEAR(cod, 0.985, 0.05);
+}
+
+#ifndef NDEBUG
+TEST(OLSTest, MalformedData) {
+ Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 4}};
+ Eigen::VectorXd y{{4}, {5}};
+ EXPECT_DEATH(sysid::OLS(X, y), "");
+}
+#endif
diff --git a/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp
new file mode 100644
index 0000000..e7b662f
--- /dev/null
+++ b/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp
@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "sysid/analysis/TrackWidthAnalysis.h"
+
+TEST(TrackWidthAnalysisTest, Calculate) {
+ double result = sysid::CalculateTrackWidth(-0.5386, 0.5386, 90_deg);
+ EXPECT_NEAR(result, 0.6858, 1E-4);
+}
