Squashed 'third_party/allwpilib/' changes from f1a82828fe..ce550705d7
ce550705d7 [ntcore] Fix client "received unknown id -1" (#6186)
3989617bde [ntcore] NetworkTable::GetStruct: Add I template param (#6183)
f1836e1321 [fieldImages] Fix 2024 field json (#6179)
d05f179a9a [build] Fix running apriltagsvision Java example (#6173)
b1b03bed85 [wpilib] Update MotorControllerGroup deprecation message (#6171)
fa63fbf446 LICENSE.md: Bump year to 2024 (#6169)
4809f3d0fc [apriltag] Add 2024 AprilTag locations (#6168)
dd90965362 [wpiutil] Fix RawFrame.setInfo() NPE (#6167)
8659372d08 [fieldImages] Add 2024 field image (#6166)
a2e4d0b15d [docs] Fix docs for SysID routine (#6164)
0a46a3a618 [wpilib] Make ADXL345 default I2C address public (#6163)
7c26bc70ab [sysid] Load DataLog files directly for analysis (#6103)
f94e3d81b9 [docs] Fix SysId routine JavaDoc warnings (#6159)
6bed82a18e [wpilibc] Clean up C++ SysId routine (#6160)
4595f84719 [wpilib] Report LiveWindow-enabled-in-test (#6158)
707cb06105 [wpilib] Add SysIdRoutine logging utility and command factory (#6033)
3e40b9e5da [wpilib] Correct SmartDashboard usage reporting (#6157)
106518c3f8 [docs] Fix wpilibj JavaDoc warnings (#6154)
19cb2a8eb4 [wpilibj] Make class variables private to match C++ (#6153)
13f4460e00 [docs] Add missing docs to enum fields (NFC) (#6150)
4210f5635d [docs] Fix warnings about undocumented default constructors (#6151)
0f060afb55 [ntcore] Disable WebSocket fragmentation (#6149)
f29a7d2e50 [docs] Add missing JavaDocs (#6146)
6e58db398d [commands] Make Java fields private (#6148)
4ac0720385 [build] Clean up CMake files (#6141)
44db3e0ac0 [sysid] Make constexpr variables outside class scope inline (#6145)
73c7d87db7 [glass] NTStringChooser: Properly set retained (#6144)
25636b712f [build] Remove unnecessary native dependencies in wpilibjExamples (#6143)
01fb98baaa [docs] Add Missing JNI docs from C++ (NFC) (#6139)
5c424248c4 [wpilibj] Remove unused AnalogTriggerException (#6142)
c486972c55 [wpimath] Make ExponentialProfile.State mutable (#6138)
783acb9b72 [wpilibj] Store long preferences as integers (#6136)
99ab836894 [wpiutil] Add missing JavaDocs (NFC) (#6132)
ad0859a8c9 [docs] Add missing JavaDocs (#6125)
5579219716 [docs] Exclude quickbuf files and proto/struct packages from doclint (#6128)
98f06911c7 [sysid] Use eigenvector component instead of eigenvalue for fit quality check (#6131)
e1d49b975c [wpimath] Add LinearFilter reset() overload to initialize input and output buffers (#6133)
8a0bf2b7a4 [hal] Add CANAPITypes to java (#6121)
91d8837c11 [wpilib] Make protected fields in accelerometers/gyros private (#6134)
e7c9f27683 [wpilib] Add functional interface equivalents to MotorController (#6053)
8aca706217 [glass] Add type information to SmartDashboard menu (#6117)
7d3e4ddba9 [docs] Add warning about using user button to docs (NFC) (#6129)
ec3cb3dcba [build] Disable clang-tidy warning about test case names (#6127)
495585b25d [examples] Update april tag family to 36h11 (#6126)
f9aabc5ab2 [wpilib] Throw early when EventLoop is modified while running (#6115)
c16946c0ec [hal] Add CANJNI docs (NFC) (#6120)
b7f4eb2811 [doc] Update maven artifacts for units and apriltags (NFC) (#6123)
f419a62b38 [doc] Update maintainers.md (NFC) (#6124)
938bf45fd9 [wpiutil] Remove type param from ProtobufSerializable and StructSerializable (#6122)
c34debe012 [docs] Link to external OpenCV docs (#6119)
07183765de [hal] Fix formatting of HAL_ENUM enums (NFC) (#6114)
af46034b7f [wpilib] Document only first party controllers are guaranteed to have correct mapping (#6112)
636ef58d94 [hal] Properly error check readCANStreamSession (#6108)
cc631d2a69 [build] Fix generated source set location in the HAL (#6113)
09f76b32c2 [wpimath] Compile with UTF-8 encoding (#6111)
47c5fd8620 [sysid] Check data quality before OLS (#6110)
24a76be694 [hal] Add method to detect if the CAN Stream has overflowed (#6105)
9333951736 [hal] Allocate CANStreamMessage in JNI if null (#6107)
6a2d3c30a6 [wpiutil] Struct: Add info template parameter pack (#6086)
e07de37e64 [commands] Mark ParallelDeadlineGroup.setDeadline() final (#6102)
141241d2d6 [wpilib] Fix usage reporting for static classes (#6090)
f2c2bab7dc [sysid] Fix adjusted R² calculation (#6101)
5659038443 [wpiutil,cscore,apriltag] Fix RawFrame (#6098)
8aeee03626 [commands] Improve error message when composing commands twice in same composition (#6091)
55508706ff [wpiutil,cscore] Move VideoMode.PixelFormat to wpiutil (#6097)
ab78b930e9 [wpilib] ADIS16470: Add access to all 3 axes (#6074)
795d4be9fd [wpilib] Fix precision issue in Color round-and-clamp (#6100)
7aa9ad44b8 [commands] Deprecate C++ TransferOwnership() (#6095)
92c81d0791 [ci] Update pregenerate workflow to actions/checkout@v4 (#6094)
1ce617be07 [ci] Update artifact actions to v4 (#6092)
2441b57156 [wpilib] Add PWMSparkFlex MotorController (#6089)
21d1972d7a [wpiutil] DataLog: Ensure file is written on shutdown (#6087)
c29e8c66cf [wpiutil] DataLog: Fix UB in AppendImpl (#6088)
ab309e34ef [glass] Fix order of loading window settings (#6056)
22a322c9f3 [wpimath] Report error on negative PID gains (#6055)
1dba26c937 [wpilib] Add method to get breaker fault at a specific channel in PowerDistribution[Sticky]Faults (#5521)
ef1cb3f41e [commands] Fix compose-while-scheduled issue and test all compositions (#5581)
aeb1a4aa33 [wpiutil] Add serializable marker interfaces (#6060)
c1178d5add [wpilib] Add StadiaController and command wrapper (#6083)
4e4a468d4d [wpimath] Make feedforward classes throw exceptions for negative Kv or Ka (#6084)
d1793f077d [build] cmake: Add NO_WERROR option to disable -Werror (#6071)
43fb6e9f87 [glass] Add Profiled PID controller support & IZone Support (#5959)
bcef6c5398 [apriltag] Fix Java generation functions (#6063)
4059e0cd9f [hal,wpilib] Add function to control "Radio" LED (#6073)
0b2cfb3abc [dlt] Change datalogtool default folder to logs folder (#6079)
df5e439b0c [wpilib] PS4Controller: enable usage reporting (#6081)
0ff7478968 [cscore] Fix RawFrame class not being loaded in JNI (#6077)
6f23d32fe1 [wpilib] AddressableLED: Update warning about single driver (NFC) (#6069)
35a1c52788 [build] Upgrade quickbuf to 1.3.3 (#6072)
e4e2bafdb1 [sysid] Document timestamp units (#6065)
3d201c71f7 [ntcore] Fix overlapping subscriber handling (#6067)
f02984159f [glass] Check for null entries when updating struct/proto (#6059)
a004c9e05f [commands] SubsystemBase: allow setting name in constructor (#6052)
0b4c6a1546 [wpimath] Add more docs to SimulatedAnnealing (NFC) (#6054)
ab15dae887 [wpilib] ArcadeDrive: Fix max output handling (#6051)
9599c1f56f [hal] Add usage reporting ids from 2024v2 image (#6041)
f87c64af8a [wpimath] MecanumDriveWheelSpeeds: Fix desaturate() (#6040)
8798700cec [wpilibcExamples] Add inline specifier to constexpr constants (#6049)
85c9ae6eff [wpilib] Fix PS5 Controller mappings (#6050)
7c8b7a97ad [wpiutil] Zero out roborio system timestamp (#6042)
d9b504bc84 [wpilib] DataLogManager: Change sim location to logs subdir (#6039)
906b810136 [build] cmake: Fix ntcore generated header install (#6038)
56e5b404d1 Update to final 2024 V2 image (#6034)
8723ee5c39 [ntcore] Add cached topic property (#5494)
192a28af47 Fix JDK 21 warnings (#6028)
d40bdd70ba [build] Upgrade to spotbugs Gradle plugin 6.0.2 (#6027)
7bfadf32e5 [wpilibj] Joystick: make remainder of get axis methods final (#6024)
a770110438 [commands] CommandCompositionError: Include stacktrace of original composition (#5984)
54a55b8b53 [wpiutil,hal] Update image; init Rio Now() HMB with a FPGA session (#6016)
7d4e515a6b [wpimath] Simplify calculation of C for DARE precondition (#6022)
5200316c14 [ntcore] Update transmit period on topic add/remove (#6021)
ddf79a25d4 [wpiunits] Overload Measure.per(Time) to return Measure<Velocity> (#6018)
a71adef316 [wpiutil] Clean up circular_buffer iterator syntax (#6020)
39a0bf4b98 [examples] Call resetOdometry() when controller command is executed (#5905)
f5fc101fda [build] cmake: Export jars and clean up jar installs (#6014)
38bf024c96 [build] Update to Gradle 8.5 (#6007)
9d11544c18 [wpimath] Rotate traveling salesman solution so input and solution have same initial pose (#6015)
28deba20f5 [wpimath] Commit generated quickbuf Java files (#5994)
c2971c0bb3 [build] cmake: Export apriltag and wpimath (#6012)
41cfc961e4 gitattributes: Add linguist-generated locations (#6004)
14c3ade155 [wpimath] Struct cleanup (#6011)
90757b9e90 [wpilib] Make Color::HexString() constexpr (#5985)
2676b77873 Fix compilation issues that occur when building with bazel (#6008)
d32c10487c [examples] Update C++ examples to use CommandPtr (#5988)
9bc5fcf886 [build] cmake: Default WITH_JAVA_SOURCE to WITH_JAVA (#6005)
d431abba3b [upstream_utils] Fix GCEM namespace usage and add hypot(x, y, z) (#6002)
2bb1409b82 Clean up Java style (#5990)
66172ab288 Remove submodule (#6003)
e8f8c0ceb0 [upstream_utils] Update to latest Eigen HEAD (#5996)
890992a849 [hal] Commit generated usage reporting files (#5993)
a583ca01e1 [wpiutil] Change Struct to allow non-constexpr implementation (#5992)
ca272de400 [build] Fix Gradle compile_commands.json and clang-tidy warnings (#5977)
76ae090570 [wpiutil] type_traits: Add is_constexpr() (#5997)
5172ab8fd0 [commands] C++ CommandPtr: Prevent null initialization (#5991)
96914143ba [build] Bump native-utils to fix compile_commands.json (#5989)
464e6121ef [ci] Report failed status to Azure on failed tests (#2654)
5dad46cd45 [wpimath] Commit generated files (#5986)
54ab65a63a [ntcore] Commit generated files (#5962)
7ed900ae3a [wpilib] Add hex string constructor to Color and Color8Bit (#5063)
74b85b76a9 [wpimath] Make gcem call std functions if not constant-evaluated (#5983)
30816111db [wpimath] Fix TimeInterpolatableBuffer crash (#5972)
5cc923de33 [wpilib] DataLogManager: Use logs subdirectory on USB drives (#5975)
1144115da0 [commands] Add GetName to Subsystem, use in Scheduler tracer epochs (#5836)
ac7d726ac3 [wpimath] Add simulated annealing (#5961)
e09be72ee0 [wpimath] Remove unused SimpleMatrixUtils class (#5979)
0f9ebe92d9 [wpimath] Add generic circular buffer class to Java (#5969)
9fa28eb07a [ci] Bump actions/checkout to v4 (#5736)
ca684ac207 [hal] Add capability to read power distribution data as a stream (#4983)
51eecef2bd [wpimath] Optimize 2nd derivative of quintic splines (#3292)
4fcf0b25a1 [build] Apply a formatter for CMake files (#5973)
9b8011aa67 [build] Pin wpiformat version (#5982)
e00a0e84c1 [build] cmake: fix protobuf dependency finding for certain distributions (#5981)
23dd591394 [upstream_utils] Remove libuv patch that adjusts whitespace (#5976)
b0719942f0 [wpiutil] Timestamp: Report errors on Rio HMB init failure (#5974)
7bc89c4322 [wpilib] Update getAlliance() docs (NFC) (#5971)
841ea682d1 [upstream_utils] Upgrade to LLVM 17.0.5 (#5970)
a74db52dae [cameraserver] Add getVideo() pixelFormat overload (#5966)
a7eb422662 [build] Update native utils for new compile commands files (#5968)
544b231d4d [sysid] Add missing cassert include (#5967)
31cd015970 [wpimath] Add SysId doc links to LinearSystemId in C++ (NFC) (#5960)
9280054eab Revert "[build] Export wpimath protobuf symbols (#5952)"
2aba97c610 Export pb files from wpimath
c80b2d2017 [build] Export wpimath protobuf symbols (#5952)
3c0652c18a [cscore] Replace CS_PixelFormat with WPI_PixelFormat (#5954)
95716eb0cb [wpiunits] Documentation improvements (#5932)
423fd75fa8 [wpilib] Default LiveWindowEnabledInTest to false (#5950)
dfdea9c992 [wpimath] Make KalmanFilter variant for asymmetric updates (#5951)
ca81ced409 [wpiutil] Move RawFrame to wpiutil; add generation of RawFrame for AprilTags (#5923)
437cc91af5 [cscore] CvSink: Allow specifying output PixelFormat (#5943)
25b7dca46b [build] Remove CMake flat install option (#5944)
bb05e20247 [wpimath] Add protobuf/struct for trivial types (#5935)
35744a036e [wpimath] Move struct/proto classes to separate files (#5918)
80d7ad58ea [build] Declare platform launcher dependency explicitly (#5909)
f8d983b154 [ntcore] Protobuf/Struct: Use atomic_bool instead of atomic_flag (#5946)
4a44210ee3 [ntcore] NetworkTableInstance: Suppress unused lambda capture warning (#5947)
bdc8620d55 [upstream_utils] Fix fmt compilation errors on Windows (#5948)
0ca1e9b5f9 [wpimath] Add basic wpiunits support (#5821)
cc30824409 [ntcore] Increase client meta-topic decoding limit (#5934)
b1fad062f7 [wpilib] Use RKDP in DifferentialDrivetrainSim (#5931)
ead9ae5a69 [build] Add generateProto dependency to test and dev (#5933)
cfbff32185 [wpiutil] timestamp: Fix startup race on Rio (#5930)
7d90d0bcc3 [wpimath] Clean up StateSpaceUtil (#5891)
7755e45aac [build] Add generated protobuf headers to C++ test include path (#5926)
3985c031da [ntcore] ProtobufSubscriber: Fix typos (#5928)
7a87fe4b60 [ntcore] ProtobufSubscriber: Make mutex and msg mutable (#5927)
09f3ed6a5f [commands] Add static Trigger factories for robot mode changes (#5902)
79dd795bc0 [wpimath] Clean up VecBuilder and MatBuilder (#5906)
e117274a67 [wpilib] Change default Rio log dir from /home/lvuser to /home/lvuser/logs (#5899)
a8b80ca256 [upstream_utils] Update to libuv 1.47.0 (#5889)
b3a9c3e96b [build] Bump macOS deployment target to 12 (#5890)
0f8129677b [build] Distribute wpimath protobuf headers (#5925)
d105f9e3e9 [wpiutil] ProtobufBuffer: Fix buffer reallocation (#5924)
c5f2f6a0fb [fieldImages] Fix typo in field images artifact name (#5922)
c1a57e422a [commands] Clean up make_vector.h (#5917)
78ebc6e9ec [wpimath] change G to gearing in LinearSystemId factories (#5834)
9ada181866 [hal] DriverStation.h: Add stddef.h include (#5897)
95fa5ec72f [wpilibc,ntcoreffi] DataLogManager: join on Stop() call (#5910)
b6f2d3cc14 [build] Remove usage of Version.parse (#5911)
cc2cbeb04c [examples] Replace gyro rotation with poseEstimator rotation (#5900)
fa6b171e1c [wpiutil] Suppress protobuf warning false positives on GCC 13 (#5907)
d504639bbe [apriltag] Improve AprilTag docs (#5895)
3a1194be40 Replace static_cast<void>() with [[maybe_unused]] attribute (#5892)
70392cbbcb [build] cmake: Add protobuf dependency to wpiutil-config (#5886)
17c1bd5a83 [ntcore] Use json_fwd (#5881)
e69a9efeba [wpilibcExamples] Match array parameter bounds (#5880)
14dcd0d26f Use char instead of uint8_t for json::parse (#5877)
ec1d261984 [hal] Fix garbage data for match info before DS connection (#5879)
63dbf5c614 [wpiutil] MemoryBuffer: Fix normal read and file type check (#5875)
b2e7be9250 [ntcore] Only datalog meta-topics if specifically requested (#5873)
201a42a3cd [wpimath] Reorder TrapezoidProfile.calculate() arguments (#5874)
04a781b4d7 [apriltag] Add GetTags to C++ version of AprilTagFieldLayout (#5872)
87a8a1ced4 [docs] Exclude eigen and protobuf from doxygen (#5871)
git-subtree-dir: third_party/allwpilib
git-subtree-split: ce550705d7cdab117c0153a202973fc026a81274
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ic8645d0551d62b411b0a816c493f0f33291896a1
diff --git a/sysid/CMakeLists.txt b/sysid/CMakeLists.txt
index 31fbe08..e8555e8 100644
--- a/sysid/CMakeLists.txt
+++ b/sysid/CMakeLists.txt
@@ -10,7 +10,7 @@
file(GLOB_RECURSE sysid_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
-if (WIN32)
+if(WIN32)
set(sysid_rc src/main/native/win/sysid.rc)
elseif(APPLE)
set(MACOSX_BUNDLE_ICON_FILE sysid.icns)
@@ -19,23 +19,28 @@
endif()
add_executable(sysid ${sysid_src} ${sysid_resources_src} ${sysid_rc} ${APP_ICON_MACOSX})
+if(MSVC)
+ target_compile_options(sysid PRIVATE /utf-8)
+endif()
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)
+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)
+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)
+ if(MSVC)
+ target_compile_options(sysid_test PRIVATE /utf-8)
+ endif()
+ 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/build.gradle b/sysid/build.gradle
index e6709fc..b9689e9 100644
--- a/sysid/build.gradle
+++ b/sysid/build.gradle
@@ -102,8 +102,8 @@
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: ':wpiutil', library: 'wpiutil', linkage: 'static'
lib project: ':wpigui', library: 'wpigui', linkage: 'static'
nativeUtils.useRequiredLibrary(it, 'imgui')
if (it.targetPlatform.operatingSystem.isWindows()) {
@@ -144,8 +144,8 @@
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: ':wpiutil', library: 'wpiutil', linkage: 'static'
lib project: ':wpigui', library: 'wpigui', linkage: 'static'
nativeUtils.useRequiredLibrary(it, 'imgui')
if (it.targetPlatform.operatingSystem.isWindows()) {
diff --git a/sysid/docs/data-collection.md b/sysid/docs/data-collection.md
index 9d28a15..538aff4 100644
--- a/sysid/docs/data-collection.md
+++ b/sysid/docs/data-collection.md
@@ -15,7 +15,7 @@
## 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).
+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). All timestamps must be in seconds.
### Non-Drivetrain Mechanisms
diff --git a/sysid/docs/arm-ols-with-angle-offset.md b/sysid/docs/ols-derivations.md
similarity index 64%
rename from sysid/docs/arm-ols-with-angle-offset.md
rename to sysid/docs/ols-derivations.md
index ecb5c43..53a8eb2 100644
--- a/sysid/docs/arm-ols-with-angle-offset.md
+++ b/sysid/docs/ols-derivations.md
@@ -1,28 +1,20 @@
-# Arm OLS with angle offset
+# OLS derivations
-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.
+## Simple/drivetrain
+
+Here's the ODE for a drivetrain.
```
-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)
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
```
-## OLS
+### OLS setup
-Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`.
+Let `α = -Kv/Ka`, `β = 1/Ka`, and `γ = -Ks/Ka`.
```
-dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
+dx/dt = αx + βu + γ sgn(x)
```
-### Ks, Kv, Ka
+### Feedforward gains
Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
```
@@ -31,7 +23,71 @@
Ka = 1/β
```
-### Kg
+## Elevator
+
+Here's the ODE for an elevator.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka
+```
+
+### OLS setup
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, and `δ = -Kg/Ka`.
+```
+dx/dt = αx + βu + γ sgn(x) + δ
+```
+
+### Feedforward gains
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, `Ka`, and `Kg`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+Kg = −δ/β
+```
+
+## Arm
+
+Here's the ODE for an arm:
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle)
+```
+
+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 setup
+
+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)
+```
+
+### Feedforward gains: Ks, Kv, Ka
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+```
+
+### Feedforward gains: 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
@@ -44,14 +100,12 @@
δ²+ε² = (Kg/Ka)² (1)
δ²+ε² = (Kg/Ka)²
√(δ²+ε²) = Kg/Ka
-√(δ²+ε²) = Kg β
-Kg = √(δ²+ε²)/β
+hypot(δ, ε) = Kg/Ka
+hypot(δ, ε) = Kg β
+Kg = hypot(δ, ε)/β
```
-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
+### Feedforward gains: 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
diff --git a/sysid/src/main/generate/WPILibVersion.cpp.in b/sysid/src/main/generate/WPILibVersion.cpp.in
index b0a4490..cfe2441 100644
--- a/sysid/src/main/generate/WPILibVersion.cpp.in
+++ b/sysid/src/main/generate/WPILibVersion.cpp.in
@@ -1,4 +1,4 @@
-/*
+/**
* 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.
*/
diff --git a/sysid/src/main/native/cpp/App.cpp b/sysid/src/main/native/cpp/App.cpp
index 947ea43..4a60f79 100644
--- a/sysid/src/main/native/cpp/App.cpp
+++ b/sysid/src/main/native/cpp/App.cpp
@@ -24,21 +24,20 @@
#include <wpigui_openurl.h>
#include "sysid/view/Analyzer.h"
-#include "sysid/view/JSONConverter.h"
-#include "sysid/view/Logger.h"
+#include "sysid/view/DataSelector.h"
+#include "sysid/view/LogLoader.h"
#include "sysid/view/UILayout.h"
namespace gui = wpi::gui;
static std::unique_ptr<glass::WindowManager> gWindowManager;
-glass::Window* gLoggerWindow;
+glass::Window* gLogLoaderWindow;
+glass::Window* gDataSelectorWindow;
glass::Window* gAnalyzerWindow;
glass::Window* gProgramLogWindow;
static glass::MainMenuBar gMainMenu;
-std::unique_ptr<sysid::JSONConverter> gJSONConverter;
-
glass::LogData gLog;
wpi::Logger gLogger;
@@ -103,11 +102,23 @@
gWindowManager = std::make_unique<glass::WindowManager>(storage);
gWindowManager->GlobalInit();
- gLoggerWindow = gWindowManager->AddWindow(
- "Logger", std::make_unique<sysid::Logger>(storage, gLogger));
+ auto logLoader = std::make_unique<sysid::LogLoader>(storage, gLogger);
+ auto dataSelector = std::make_unique<sysid::DataSelector>(storage, gLogger);
+ auto analyzer = std::make_unique<sysid::Analyzer>(storage, gLogger);
- gAnalyzerWindow = gWindowManager->AddWindow(
- "Analyzer", std::make_unique<sysid::Analyzer>(storage, gLogger));
+ logLoader->unload.connect([ds = dataSelector.get()] { ds->Reset(); });
+ dataSelector->testdata = [_analyzer = analyzer.get()](auto data) {
+ _analyzer->m_data = data;
+ _analyzer->AnalyzeData();
+ };
+
+ gLogLoaderWindow =
+ gWindowManager->AddWindow("Log Loader", std::move(logLoader));
+
+ gDataSelectorWindow =
+ gWindowManager->AddWindow("Data Selector", std::move(dataSelector));
+
+ gAnalyzerWindow = gWindowManager->AddWindow("Analyzer", std::move(analyzer));
gProgramLogWindow = gWindowManager->AddWindow(
"Program Log", std::make_unique<glass::LogView>(&gLog));
@@ -115,10 +126,16 @@
// 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);
+ gLogLoaderWindow->SetDefaultPos(sysid::kLogLoaderWindowPos.x,
+ sysid::kLogLoaderWindowPos.y);
+ gLogLoaderWindow->SetDefaultSize(sysid::kLogLoaderWindowSize.x,
+ sysid::kLogLoaderWindowSize.y);
+
+ // Data selector window position/size
+ gDataSelectorWindow->SetDefaultPos(sysid::kDataSelectorWindowPos.x,
+ sysid::kDataSelectorWindowPos.y);
+ gDataSelectorWindow->SetDefaultSize(sysid::kDataSelectorWindowSize.x,
+ sysid::kDataSelectorWindowSize.y);
// Analyzer window position/size
gAnalyzerWindow->SetDefaultPos(sysid::kAnalyzerWindowPos.x,
@@ -133,8 +150,6 @@
sysid::kProgramLogWindowSize.y);
gProgramLogWindow->DisableRenamePopup();
- gJSONConverter = std::make_unique<sysid::JSONConverter>(gLogger);
-
// Configure save file.
gui::ConfigurePlatformSaveFile("sysid.ini");
@@ -157,15 +172,6 @@
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(
@@ -178,19 +184,6 @@
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;
diff --git a/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
index b15cae8..be88b57 100644
--- a/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
+++ b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
@@ -5,24 +5,39 @@
#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/MathExtras.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;
+static double Lerp(units::second_t time,
+ std::vector<MotorData::Run::Sample<double>>& data) {
+ auto next = std::find_if(data.begin(), data.end(), [&](const auto& entry) {
+ return entry.time > time;
+ });
+
+ if (next == data.begin()) {
+ next++;
+ }
+
+ if (next == data.end()) {
+ next--;
+ }
+
+ const auto prev = next - 1;
+
+ return wpi::Lerp(prev->measurement, next->measurement,
+ (time - prev->time) / (next->time - prev->time));
+}
+
/**
* Converts a raw data vector into a PreparedData vector with only the
* timestamp, voltage, position, and velocity fields filled out.
@@ -38,18 +53,25 @@
*
* @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) {
+static std::vector<PreparedData> ConvertToPrepared(const MotorData& 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]}});
+ // assume we've selected down to a single contiguous run by this point
+ auto run = data.runs[0];
+
+ for (int i = 0; i < static_cast<int>(run.voltage.size()) - 1; ++i) {
+ const auto& currentVoltage = run.voltage[i];
+ const auto& nextVoltage = run.voltage[i + 1];
+
+ auto currentPosition = Lerp(currentVoltage.time, run.position);
+
+ auto currentVelocity = Lerp(currentVoltage.time, run.velocity);
+
+ prepared.emplace_back(PreparedData{currentVoltage.time,
+ currentVoltage.measurement.value(),
+ currentPosition, currentVelocity,
+ nextVoltage.time - currentVoltage.time});
}
+
return prepared;
}
@@ -62,18 +84,16 @@
*
* @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) {
+static void CopyRawData(wpi::StringMap<MotorData>* dataset) {
auto& data = *dataset;
// Loads the Raw Data
for (auto& it : data) {
auto key = it.first();
- auto& dataset = it.getValue();
+ auto& motorData = it.getValue();
if (!wpi::contains(key, "raw")) {
- data[fmt::format("raw-{}", key)] = dataset;
- data[fmt::format("original-raw-{}", key)] = dataset;
+ data[fmt::format("raw-{}", key)] = motorData;
+ data[fmt::format("original-raw-{}", key)] = motorData;
}
}
}
@@ -94,416 +114,73 @@
}
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);
+ CopyRawData(&m_data.motorData);
WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct.");
// Convert data to PreparedData structs
- for (auto& it : data) {
+ for (auto& it : m_data.motorData) {
auto key = it.first();
- preparedData[key] =
- ConvertToPrepared<4, kTimeCol, kVoltageCol, kPosCol, kVelCol>(
- data[key]);
+ preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
+ WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
}
// 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"]);
+ m_originalDataset =
+ CombineDatasets(preparedData["original-raw-quasistatic-forward"],
+ preparedData["original-raw-quasistatic-reverse"],
+ preparedData["original-raw-dynamic-forward"],
+ preparedData["original-raw-dynamic-reverse"]);
WPI_INFO(m_logger, "{}", "Initial trimming and filtering.");
sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
m_velocityDelays, m_minStepTime, m_maxStepTime,
- m_unit);
+ m_data.distanceUnit);
+
+ WPI_INFO(m_logger, "{}", m_minStepTime);
+ WPI_INFO(m_logger, "{}", m_maxStepTime);
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"]);
+ m_rawDataset = CombineDatasets(preparedData["raw-quasistatic-forward"],
+ preparedData["raw-quasistatic-reverse"],
+ preparedData["raw-dynamic-forward"],
+ preparedData["raw-dynamic-reverse"]);
// 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_filteredDataset = CombineDatasets(
+ preparedData["quasistatic-forward"], preparedData["quasistatic-reverse"],
+ preparedData["dynamic-forward"], preparedData["dynamic-reverse"]);
- 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};
+ m_startTimes = {preparedData["raw-quasistatic-forward"][0].timestamp,
+ preparedData["raw-quasistatic-reverse"][0].timestamp,
+ preparedData["raw-dynamic-forward"][0].timestamp,
+ preparedData["raw-dynamic-reverse"][0].timestamp};
}
AnalysisManager::AnalysisManager(Settings& settings, wpi::Logger& logger)
- : m_logger{logger},
- m_settings{settings},
- m_type{analysis::kSimple},
- m_unit{"Meters"},
- m_factor{1} {}
+ : m_logger{logger}, m_settings{settings} {}
-AnalysisManager::AnalysisManager(std::string_view path, Settings& settings,
+AnalysisManager::AnalysisManager(TestData data, 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);
-
+ : m_data{std::move(data)}, m_logger{logger}, m_settings{settings} {
// 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, "Preparing {} data", m_data.mechanismType.name);
+
+ PrepareGeneralData();
+
WPI_INFO(m_logger, "{}", "Finished Preparing Data");
}
@@ -515,12 +192,13 @@
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& ff = sysid::CalculateFeedforwardGains(
+ GetFilteredData(), m_data.mechanismType, false);
+ FeedforwardGains ffGains = {ff};
- const auto& Ks = std::get<0>(ff)[0];
- const auto& Kv = std::get<0>(ff)[1];
- const auto& Ka = std::get<0>(ff)[2];
+ const auto& Ks = ff.coeffs[0];
+ const auto& Kv = ff.coeffs[1];
+ const auto& Ka = ff.coeffs[2];
if (Ka <= 0 || Kv < 0) {
throw InvalidDataError(
@@ -542,27 +220,20 @@
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);
+ m_settings.convertGainsToEncTicks ? m_settings.gearing * m_settings.cpr
+ : 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);
+ m_settings.convertGainsToEncTicks ? m_settings.gearing * m_settings.cpr
+ : 1);
}
return fb;
}
-void AnalysisManager::OverrideUnits(std::string_view unit,
- double unitsPerRotation) {
- m_unit = unit;
- m_factor = unitsPerRotation;
+void AnalysisManager::OverrideUnits(std::string_view unit) {
+ m_data.distanceUnit = unit;
}
-void AnalysisManager::ResetUnitsFromJSON() {
- m_unit = m_json.at("units").get<std::string>();
- m_factor = m_json.at("unitsPerRotation").get<double>();
-}
+void AnalysisManager::ResetUnitsFromJSON() {}
diff --git a/sysid/src/main/native/cpp/analysis/AnalysisType.cpp b/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
index 18b461f..6ef27c9 100644
--- a/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
+++ b/sysid/src/main/native/cpp/analysis/AnalysisType.cpp
@@ -7,12 +7,6 @@
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;
}
diff --git a/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
index b7a9fce..110ff94 100644
--- a/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
+++ b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
@@ -4,8 +4,13 @@
#include "sysid/analysis/FeedforwardAnalysis.h"
+#include <array>
+#include <bitset>
#include <cmath>
+#include <Eigen/Eigenvalues>
+#include <fmt/format.h>
+#include <fmt/ranges.h>
#include <units/math.h>
#include <units/time.h>
@@ -13,10 +18,25 @@
#include "sysid/analysis/FilteringUtils.h"
#include "sysid/analysis/OLS.h"
-using namespace sysid;
+namespace sysid {
/**
- * Populates OLS data for (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ).
+ * Populates OLS data for the following models:
+ *
+ * Simple, Drivetrain, DrivetrainAngular:
+ *
+ * (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ)
+ *
+ * Elevator:
+ *
+ * (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + δ
+ *
+ * Arm:
+ *
+ * (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + δ cos(angle) + ε sin(angle)
+ *
+ * OLS performs best with the noisiest variable as the dependent variable, so we
+ * regress acceleration in terms of the other variables.
*
* @param d List of characterization data.
* @param type Type of system being identified.
@@ -27,36 +47,136 @@
const AnalysisType& type,
Eigen::Block<Eigen::MatrixXd> X,
Eigen::VectorBlock<Eigen::VectorXd> y) {
+ // Fill in X and y row-wise
for (size_t sample = 0; sample < d.size(); ++sample) {
const auto& pt = d[sample];
- // Add the velocity term (for α)
+ // Set the velocity term (for α)
X(sample, 0) = pt.velocity;
- // Add the voltage term (for β)
+ // Set the voltage term (for β)
X(sample, 1) = pt.voltage;
- // Add the intercept term (for γ)
+ // Set the intercept term (for γ)
X(sample, 2) = std::copysign(1, pt.velocity);
- // Add test-specific variables
+ // Set test-specific variables
if (type == analysis::kElevator) {
- // Add the gravity term (for Kg)
+ // Set the gravity term (for δ)
X(sample, 3) = 1.0;
} else if (type == analysis::kArm) {
- // Add the cosine and sine terms (for Kg)
+ // Set the cosine and sine terms (for δ and ε)
X(sample, 3) = pt.cos;
X(sample, 4) = pt.sin;
}
- // Add the dependent variable (acceleration)
+ // Set the dependent variable (acceleration)
y(sample) = pt.acceleration;
}
}
-std::tuple<std::vector<double>, double, double>
-sysid::CalculateFeedforwardGains(const Storage& data,
- const AnalysisType& type) {
+/**
+ * Throws an InsufficientSamplesError if the collected data is poor for OLS.
+ *
+ * @param X The collected data in matrix form for OLS.
+ * @param type The analysis type.
+ */
+static void CheckOLSDataQuality(const Eigen::MatrixXd& X,
+ const AnalysisType& type) {
+ Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigSolver{X.transpose() * X};
+ const Eigen::VectorXd& eigvals = eigSolver.eigenvalues();
+ const Eigen::MatrixXd& eigvecs = eigSolver.eigenvectors();
+
+ // Bits are Ks, Kv, Ka, Kg, offset
+ std::bitset<5> badGains;
+
+ constexpr double threshold = 10.0;
+
+ // For n x n matrix XᵀX, need n nonzero eigenvalues for good fit
+ for (int row = 0; row < eigvals.rows(); ++row) {
+ // Find row of eigenvector with largest magnitude. This determines the
+ // primary regression variable that corresponds to the eigenvalue.
+ int maxIndex;
+ double maxCoeff = eigvecs.col(row).cwiseAbs().maxCoeff(&maxIndex);
+
+ // Check whether the eigenvector component along the regression variable's
+ // direction is below the threshold. If it is, the regression variable's fit
+ // is bad.
+ if (std::abs(eigvals(row) * maxCoeff) <= threshold) {
+ // Fit for α is bad
+ if (maxIndex == 0) {
+ // Affects Kv
+ badGains.set(1);
+ }
+
+ // Fit for β is bad
+ if (maxIndex == 1) {
+ // Affects all gains
+ badGains.set();
+ break;
+ }
+
+ // Fit for γ is bad
+ if (maxIndex == 2) {
+ // Affects Ks
+ badGains.set(0);
+ }
+
+ // Fit for δ is bad
+ if (maxIndex == 3) {
+ if (type == analysis::kElevator) {
+ // Affects Kg
+ badGains.set(3);
+ } else if (type == analysis::kArm) {
+ // Affects Kg and offset
+ badGains.set(3);
+ badGains.set(4);
+ }
+ }
+
+ // Fit for ε is bad
+ if (maxIndex == 4) {
+ // Affects Kg and offset
+ badGains.set(3);
+ badGains.set(4);
+ }
+ }
+ }
+
+ // If any gains are bad, throw an error
+ if (badGains.any()) {
+ // Create list of bad gain names
+ constexpr std::array gainNames{"Ks", "Kv", "Ka", "Kg", "offset"};
+ std::vector<std::string_view> badGainsList;
+ for (size_t i = 0; i < badGains.size(); ++i) {
+ if (badGains.test(i)) {
+ badGainsList.emplace_back(gainNames[i]);
+ }
+ }
+
+ std::string error = fmt::format("Insufficient samples to compute {}.\n\n",
+ fmt::join(badGainsList, ", "));
+
+ // If all gains are bad, the robot may not have moved
+ if (badGains.all()) {
+ error += "Either no data was collected or the robot didn't move.\n\n";
+ }
+
+ // Append guidance for fixing the data
+ error +=
+ "Ensure the data has:\n\n"
+ " * at least 2 steady-state velocity events to separate Ks from Kv\n"
+ " * at least 1 acceleration event to find Ka\n"
+ " * for elevators, enough vertical motion to measure gravity\n"
+ " * for arms, enough range of motion to measure gravity and encoder "
+ "offset\n";
+ throw InsufficientSamplesError{error};
+ }
+}
+
+OLSResult CalculateFeedforwardGains(const Storage& data,
+ const AnalysisType& type,
+ bool throwOnBadData) {
// Iterate through the data and add it to our raw vector.
const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
@@ -87,34 +207,68 @@
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);
+ // Check quality of collected data
+ if (throwOnBadData) {
+ CheckOLSDataQuality(X, type);
}
- 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)
+ std::vector<double> gains;
+ gains.reserve(X.rows());
- // Add Kg to gains list
- gains.emplace_back(std::hypot(delta, epsilon) / beta);
+ auto ols = OLS(X, y);
- // Add offset to gains list
- gains.emplace_back(std::atan2(epsilon, -delta));
+ // Calculate feedforward gains
+ //
+ // See docs/ols-derivations.md for more details.
+ {
+ // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
+ // dx/dt = αx + βu + γ sgn(x)
+
+ // α = -Kv/Ka
+ // β = 1/Ka
+ // γ = -Ks/Ka
+ double α = ols.coeffs[0];
+ double β = ols.coeffs[1];
+ double γ = ols.coeffs[2];
+
+ // Ks = -γ/β
+ // Kv = -α/β
+ // Ka = 1/β
+ gains.emplace_back(-γ / β);
+ gains.emplace_back(-α / β);
+ gains.emplace_back(1 / β);
+
+ if (type == analysis::kElevator) {
+ // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka
+ // dx/dt = αx + βu + γ sgn(x) + δ
+
+ // δ = -Kg/Ka
+ double δ = ols.coeffs[3];
+
+ // Kg = -δ/β
+ gains.emplace_back(-δ / β);
+ }
+
+ if (type == analysis::kArm) {
+ // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x)
+ // - Kg/Ka cos(offset) cos(angle) NOLINT
+ // + Kg/Ka sin(offset) sin(angle) NOLINT
+ // dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle) NOLINT
+
+ // δ = -Kg/Ka cos(offset)
+ // ε = Kg/Ka sin(offset)
+ double δ = ols.coeffs[3];
+ double ε = ols.coeffs[4];
+
+ // Kg = hypot(δ, ε)/β NOLINT
+ // offset = atan2(ε, -δ) NOLINT
+ gains.emplace_back(std::hypot(δ, ε) / β);
+ gains.emplace_back(std::atan2(ε, -δ));
+ }
}
// Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only)
- return std::tuple{gains, std::get<1>(ols), std::get<2>(ols)};
+ return OLSResult{gains, ols.rSquared, ols.rmse};
}
+
+} // namespace sysid
diff --git a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
index 6c66ef8..838968f 100644
--- a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
+++ b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
@@ -153,32 +153,24 @@
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;
- });
+ // Find maximum speed reached
+ const auto maxSpeed =
+ GetMaxSpeed(*data, [](auto&& pt) { return pt.velocity; });
+ // Find place where 90% of maximum speed exceeded
+ auto endIt =
+ std::find_if(data->begin(), data->end(), [&](const PreparedData& entry) {
+ return std::abs(entry.velocity) > maxSpeed * 0.9;
+ });
- if (endIt != data->rend()) {
- // Calculate default duration
- settings->stepTestDuration = std::min(
- endIt->timestamp - data->front().timestamp + minStepTime + 1_s,
- maxStepTime);
- } else {
- settings->stepTestDuration = maxStepTime;
- }
+ if (endIt != data->end()) {
+ settings->stepTestDuration = std::min(
+ endIt->timestamp - data->front().timestamp + minStepTime, 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 >
+ return entry.timestamp - data->front().timestamp >
settings->stepTestDuration;
});
@@ -186,6 +178,7 @@
if (maxIt != data->end()) {
data->erase(maxIt, data->end());
}
+
return std::make_tuple(minStepTime, positionDelay, velocityDelay);
}
@@ -204,6 +197,16 @@
return std::sqrt(sum / (data.size() - step));
}
+double sysid::GetMaxSpeed(
+ const std::vector<PreparedData>& data,
+ std::function<double(const PreparedData&)> accessorFunction) {
+ double max = 0.0;
+ for (size_t i = 0; i < data.size(); i++) {
+ max = std::max(max, std::abs(accessorFunction(data[i])));
+ }
+ return max;
+}
+
units::second_t sysid::GetMeanTimeDelta(const std::vector<PreparedData>& data) {
std::vector<units::second_t> dts;
@@ -301,7 +304,7 @@
auto key = it.first();
auto& dataset = it.getValue();
- if (IsRaw(key) && wpi::contains(key, "fast")) {
+ if (IsRaw(key) && wpi::contains(key, "dynamic")) {
auto duration = dataset.back().timestamp - dataset.front().timestamp;
if (duration > maxStepTime) {
maxStepTime = duration;
@@ -327,7 +330,7 @@
for (auto& it : preparedData) {
auto key = it.first();
auto& dataset = it.getValue();
- if (wpi::contains(key, "slow")) {
+ if (wpi::contains(key, "quasistatic")) {
settings->motionThreshold =
std::min(settings->motionThreshold,
GetNoiseFloor(dataset, kNoiseMeanWindow,
@@ -342,7 +345,7 @@
// Trim quasistatic test data to remove all points where voltage is zero or
// velocity < motion threshold.
- if (wpi::contains(key, "slow")) {
+ if (wpi::contains(key, "quasistatic")) {
dataset.erase(std::remove_if(dataset.begin(), dataset.end(),
[&](const auto& pt) {
return std::abs(pt.voltage) <= 0 ||
@@ -366,7 +369,7 @@
PrepareMechData(&dataset, unit);
// Trims filtered Dynamic Test Data
- if (IsFiltered(key) && wpi::contains(key, "fast")) {
+ if (IsFiltered(key) && wpi::contains(key, "dynamic")) {
// Get the filtered dataset name
auto filteredKey = RemoveStr(key, "raw-");
diff --git a/sysid/src/main/native/cpp/analysis/JSONConverter.cpp b/sysid/src/main/native/cpp/analysis/JSONConverter.cpp
deleted file mode 100644
index 9bab8c6..0000000
--- a/sysid/src/main/native/cpp/analysis/JSONConverter.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-// 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
index d095a48..5d630d0 100644
--- a/sysid/src/main/native/cpp/analysis/OLS.cpp
+++ b/sysid/src/main/native/cpp/analysis/OLS.cpp
@@ -4,45 +4,85 @@
#include "sysid/analysis/OLS.h"
-#include <tuple>
-#include <vector>
+#include <cassert>
+#include <cmath>
#include <Eigen/Cholesky>
-#include <Eigen/Core>
-using namespace sysid;
+namespace sysid {
-std::tuple<std::vector<double>, double, double> sysid::OLS(
- const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
+OLSResult 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.
+ // The linear regression model can be written as follows:
+ //
+ // y = Xβ + ε
+ //
+ // where y is the dependent observed variable, X is the matrix of independent
+ // variables, β is a vector of coefficients, and ε is a vector of residuals.
+ //
+ // We want to find the value of β that minimizes εᵀε.
+ //
+ // ε = y − Xβ
+ // εᵀε = (y − Xβ)ᵀ(y − Xβ)
+ //
+ // β̂ = argmin (y − Xβ)ᵀ(y − Xβ)
+ // β
+ //
+ // Take the partial derivative of the cost function with respect to β and set
+ // it equal to zero, then solve for β̂ .
+ //
+ // 0 = −2Xᵀ(y − Xβ̂)
+ // 0 = Xᵀ(y − Xβ̂)
+ // 0 = Xᵀy − XᵀXβ̂
+ // XᵀXβ̂ = Xᵀy
+ // β̂ = (XᵀX)⁻¹Xᵀy
- // We want to minimize u² = uᵀu = (y - Xβ)ᵀ(y - Xβ).
// β = (XᵀX)⁻¹Xᵀy
+ //
+ // XᵀX is guaranteed to be symmetric positive definite, so an LLT
+ // decomposition can be used.
+ Eigen::MatrixXd β = (X.transpose() * X).llt().solve(X.transpose() * y);
- // Calculate β that minimizes uᵀu.
- Eigen::MatrixXd beta = (X.transpose() * X).llt().solve(X.transpose() * y);
+ // Error sum of squares
+ double SSE = (y - X * β).squaredNorm();
- // 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.
+ // Sample size
+ int n = X.rows();
- // We will first calculate the sum of the squares of the error, or the
- // variation in error (SSE).
- double SSE = (y - X * beta).squaredNorm();
+ // Number of explanatory variables
+ int p = β.rows();
- int n = X.cols();
+ // Total sum of squares (total variation in y)
+ //
+ // From slide 24 of
+ // http://www.stat.columbia.edu/~fwood/Teaching/w4315/Fall2009/lecture_11:
+ //
+ // SSTO = yᵀy - 1/n yᵀJy
+ //
+ // Let J = I.
+ //
+ // SSTO = yᵀy - 1/n yᵀy
+ // SSTO = (n − 1)/n yᵀy
+ double SSTO = (n - 1.0) / n * (y.transpose() * y).value();
- // Now we will calculate the total variation in y, known as SSTO.
- double SSTO = ((y.transpose() * y) - (1.0 / n) * (y.transpose() * y)).value();
+ // R² or the coefficient of determination, which represents how much of the
+ // total variation (variation in y) can be explained by the regression model
+ double rSquared = 1.0 - SSE / SSTO;
- double rSquared = (SSTO - SSE) / SSTO;
- double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - 3.0));
+ // Adjusted R²
+ //
+ // n − 1
+ // R̅² = 1 − (1 − R²) ---------
+ // n − p − 1
+ //
+ // See https://en.wikipedia.org/wiki/Coefficient_of_determination#Adjusted_R2
+ double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - p - 1.0));
+
+ // Root-mean-square error
double RMSE = std::sqrt(SSE / n);
- return {{beta.data(), beta.data() + beta.rows()}, adjRSquared, RMSE};
+ return {{β.data(), β.data() + β.size()}, adjRSquared, RMSE};
}
+
+} // namespace sysid
diff --git a/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp b/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp
deleted file mode 100644
index ac97cdb..0000000
--- a/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp
+++ /dev/null
@@ -1,275 +0,0 @@
-// 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
index 3270918..f121ab5 100644
--- a/sysid/src/main/native/cpp/view/Analyzer.cpp
+++ b/sysid/src/main/native/cpp/view/Analyzer.cpp
@@ -28,7 +28,7 @@
using namespace sysid;
Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger)
- : m_location(""), m_logger(logger) {
+ : m_logger(logger) {
// Fill the StringMap with preset values.
m_presets["Default"] = presets::kDefault;
m_presets["WPILib (2020-)"] = presets::kWPILibNew;
@@ -48,16 +48,14 @@
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;
+ const auto& [ff] = m_manager->CalculateFeedforward();
+ m_ff = ff.coeffs;
+ m_accelRSquared = ff.rSquared;
+ m_accelRMSE = ff.rmse;
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;
@@ -81,6 +79,7 @@
}
void Analyzer::UpdateFeedbackGains() {
+ WPI_INFO(m_logger, "{}", "Updating feedback gains");
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]};
@@ -119,27 +118,9 @@
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();
}
@@ -152,38 +133,15 @@
ImGui::SameLine(width - ImGui::CalcTextSize("Reset").x);
if (ImGui::Button("Reset")) {
ResetData();
- m_state = AnalyzerState::kWaitingForJSON;
+ m_state = AnalyzerState::kWaitingForData;
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.");
- }
+ std::string(unit).c_str(), type.name);
if (ImGui::Button("Override Units")) {
ImGui::OpenPopup("Override Units");
@@ -197,24 +155,11 @@
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);
+ m_manager->OverrideUnits(unit);
PrepareData();
}
@@ -234,22 +179,21 @@
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 qp as 1/10 native distance unit
+ m_settings.lqr.qp = 0.1;
// 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: {
+ case AnalyzerState::kWaitingForData: {
ImGui::Text(
"SysId is currently in theoretical analysis mode.\n"
"To analyze recorded test data, select a "
- "data JSON.");
+ "data file (.wpilog).");
sysid::CreateTooltip(
"Theoretical feedback gains can be calculated from a "
"physical model of the mechanism being controlled. "
@@ -295,7 +239,7 @@
case AnalyzerState::kFileError: {
CreateErrorPopup(m_errorPopup, m_exception);
if (!m_errorPopup) {
- m_state = AnalyzerState::kWaitingForJSON;
+ m_state = AnalyzerState::kWaitingForData;
return;
}
break;
@@ -313,20 +257,10 @@
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() {
+ WPI_INFO(m_logger, "{}", "Preparing data");
try {
m_manager->PrepareData();
UpdateFeedforwardGains();
@@ -379,9 +313,6 @@
void Analyzer::HandleError(std::string_view msg) {
m_exception = msg;
m_errorPopup = true;
- if (m_state == AnalyzerState::kFileError) {
- m_location = "";
- }
PrepareRawGraphs();
}
@@ -458,23 +389,12 @@
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::AnalyzeData() {
+ m_manager = std::make_unique<AnalysisManager>(m_data, m_settings, m_logger);
+ PrepareData();
+ m_dataset = 0;
+ ConfigParamsOnFileSelect();
+ UpdateFeedbackGains();
}
void Analyzer::AbortDataPrep() {
@@ -625,8 +545,6 @@
"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();
@@ -790,7 +708,7 @@
IM_ARRAYSIZE(kLoopTypes))) {
m_settings.type =
static_cast<FeedbackControllerLoopType>(m_selectedLoopType);
- if (m_state == AnalyzerState::kWaitingForJSON) {
+ if (m_state == AnalyzerState::kWaitingForData) {
m_settings.preset.measurementDelay = 0_ms;
} else {
if (m_settings.type == FeedbackControllerLoopType::kPosition) {
@@ -817,7 +735,7 @@
if (m_selectedLoopType == 0) {
std::string unit;
- if (m_state != AnalyzerState::kWaitingForJSON) {
+ if (m_state != AnalyzerState::kWaitingForData) {
unit = fmt::format(" ({})", GetAbbreviation(m_manager->GetUnit()));
}
@@ -831,7 +749,7 @@
}
std::string unit;
- if (m_state != AnalyzerState::kWaitingForJSON) {
+ if (m_state != AnalyzerState::kWaitingForData) {
unit = fmt::format(" ({}/s)", GetAbbreviation(m_manager->GetUnit()));
}
diff --git a/sysid/src/main/native/cpp/view/DataSelector.cpp b/sysid/src/main/native/cpp/view/DataSelector.cpp
new file mode 100644
index 0000000..c160490
--- /dev/null
+++ b/sysid/src/main/native/cpp/view/DataSelector.cpp
@@ -0,0 +1,244 @@
+// 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/DataSelector.h"
+
+#include <fmt/format.h>
+#include <glass/support/DataLogReaderThread.h>
+#include <imgui.h>
+#include <wpi/DataLogReader.h>
+#include <wpi/Logger.h>
+#include <wpi/StringExtras.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/Storage.h"
+
+using namespace sysid;
+
+static constexpr const char* kAnalysisTypes[] = {"Elevator", "Arm", "Simple"};
+
+static bool EmitEntryTarget(const char* name, bool isString,
+ const glass::DataLogReaderEntry** entry) {
+ if (*entry) {
+ auto text =
+ fmt::format("{}: {} ({})", name, (*entry)->name, (*entry)->type);
+ ImGui::TextUnformatted(text.c_str());
+ } else {
+ ImGui::Text("%s: <none (DROP HERE)> (%s)", name,
+ isString ? "string" : "number");
+ }
+ bool rv = false;
+ if (ImGui::BeginDragDropTarget()) {
+ if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload(
+ isString ? "DataLogEntryString" : "DataLogEntry")) {
+ assert(payload->DataSize == sizeof(const glass::DataLogReaderEntry*));
+ *entry = *static_cast<const glass::DataLogReaderEntry**>(payload->Data);
+ rv = true;
+ }
+ ImGui::EndDragDropTarget();
+ }
+ return rv;
+}
+
+void DataSelector::Display() {
+ using namespace std::chrono_literals;
+
+ // building test data is modal (due to async access)
+ if (m_testdataFuture.valid()) {
+ if (m_testdataFuture.wait_for(0s) == std::future_status::ready) {
+ TestData data = m_testdataFuture.get();
+ for (auto&& motordata : data.motorData) {
+ m_testdataStats.emplace_back(
+ fmt::format("Test State: {}", motordata.first()));
+ int i = 0;
+ for (auto&& run : motordata.second.runs) {
+ m_testdataStats.emplace_back(fmt::format(
+ " Run {} samples: {} Volt {} Pos {} Vel", ++i,
+ run.voltage.size(), run.position.size(), run.velocity.size()));
+ }
+ }
+ if (testdata) {
+ testdata(std::move(data));
+ }
+ }
+ ImGui::Text("Loading data...");
+ return;
+ }
+
+ if (!m_testdataStats.empty()) {
+ for (auto&& line : m_testdataStats) {
+ ImGui::TextUnformatted(line.c_str());
+ }
+ if (ImGui::Button("Ok")) {
+ m_testdataStats.clear();
+ }
+ return;
+ }
+
+ if (EmitEntryTarget("Test State", true, &m_testStateEntry)) {
+ m_testsFuture =
+ std::async(std::launch::async, [testStateEntry = m_testStateEntry] {
+ return LoadTests(*testStateEntry);
+ });
+ }
+
+ if (!m_testStateEntry) {
+ return;
+ }
+
+ if (m_testsFuture.valid() &&
+ m_testsFuture.wait_for(0s) == std::future_status::ready) {
+ m_tests = m_testsFuture.get();
+ }
+
+ if (m_tests.empty()) {
+ if (m_testsFuture.valid()) {
+ ImGui::TextUnformatted("Reading tests...");
+ } else {
+ ImGui::TextUnformatted("No tests found");
+ }
+ return;
+ }
+
+#if 0
+ // Test filtering
+ if (ImGui::BeginCombo("Test", m_selectedTest.c_str())) {
+ for (auto&& test : m_tests) {
+ if (ImGui::Selectable(test.first.c_str(), test.first == m_selectedTest)) {
+ m_selectedTest = test.first;
+ }
+ }
+ ImGui::EndCombo();
+ }
+#endif
+
+ ImGui::Combo("Analysis Type", &m_selectedAnalysis, kAnalysisTypes,
+ IM_ARRAYSIZE(kAnalysisTypes));
+
+ // DND targets
+ EmitEntryTarget("Velocity", false, &m_velocityEntry);
+ EmitEntryTarget("Position", false, &m_positionEntry);
+ EmitEntryTarget("Voltage", false, &m_voltageEntry);
+
+ ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
+ ImGui::Combo("Units", &m_selectedUnit, kUnits, IM_ARRAYSIZE(kUnits));
+
+ ImGui::InputDouble("Velocity scaling", &m_velocityScale);
+ ImGui::InputDouble("Position scaling", &m_positionScale);
+
+ if (/*!m_selectedTest.empty() &&*/ m_velocityEntry && m_positionEntry &&
+ m_voltageEntry) {
+ if (ImGui::Button("Load")) {
+ m_testdataFuture =
+ std::async(std::launch::async, [this] { return BuildTestData(); });
+ }
+ }
+}
+
+void DataSelector::Reset() {
+ m_testsFuture = {};
+ m_tests.clear();
+ m_selectedTest.clear();
+ m_testStateEntry = nullptr;
+ m_velocityEntry = nullptr;
+ m_positionEntry = nullptr;
+ m_voltageEntry = nullptr;
+ m_testdataFuture = {};
+}
+
+DataSelector::Tests DataSelector::LoadTests(
+ const glass::DataLogReaderEntry& testStateEntry) {
+ Tests tests;
+ for (auto&& range : testStateEntry.ranges) {
+ std::string_view prevState;
+ Runs* curRuns = nullptr;
+ wpi::log::DataLogReader::iterator lastStart = range.begin();
+ for (auto it = range.begin(), end = range.end(); it != end; ++it) {
+ std::string_view testState;
+ if (it->GetEntry() != testStateEntry.entry ||
+ !it->GetString(&testState)) {
+ continue;
+ }
+
+ // track runs as iterator ranges of the same test
+ if (testState != prevState) {
+ if (curRuns) {
+ curRuns->emplace_back(lastStart, it);
+ }
+ lastStart = it;
+ }
+ prevState = testState;
+
+ if (testState == "none") {
+ curRuns = nullptr;
+ continue;
+ }
+
+ auto [testName, direction] = wpi::rsplit(testState, '-');
+ auto testIt = tests.find(testName);
+ if (testIt == tests.end()) {
+ testIt = tests.emplace(std::string{testName}, State{}).first;
+ }
+ auto stateIt = testIt->second.find(testState);
+ if (stateIt == testIt->second.end()) {
+ stateIt = testIt->second.emplace(std::string{testState}, Runs{}).first;
+ }
+ curRuns = &stateIt->second;
+ }
+
+ if (curRuns) {
+ curRuns->emplace_back(lastStart, range.end());
+ }
+ }
+ return tests;
+}
+
+template <typename T>
+static void AddSample(std::vector<MotorData::Run::Sample<T>>& samples,
+ const wpi::log::DataLogRecord& record, bool isDouble,
+ double scale) {
+ if (isDouble) {
+ double val;
+ if (record.GetDouble(&val)) {
+ samples.emplace_back(units::second_t{record.GetTimestamp() * 1.0e-6},
+ T{val * scale});
+ }
+ } else {
+ float val;
+ if (record.GetFloat(&val)) {
+ samples.emplace_back(units::second_t{record.GetTimestamp() * 1.0e-6},
+ T{static_cast<double>(val * scale)});
+ }
+ }
+}
+
+TestData DataSelector::BuildTestData() {
+ TestData data;
+ data.distanceUnit = kUnits[m_selectedUnit];
+ data.mechanismType = analysis::FromName(kAnalysisTypes[m_selectedAnalysis]);
+ bool voltageDouble = m_voltageEntry->type == "double";
+ bool positionDouble = m_positionEntry->type == "double";
+ bool velocityDouble = m_velocityEntry->type == "double";
+
+ for (auto&& test : m_tests) {
+ for (auto&& state : test.second) {
+ auto& motorData = data.motorData[state.first];
+ for (auto&& range : state.second) {
+ auto& run = motorData.runs.emplace_back();
+ for (auto&& record : range) {
+ if (record.GetEntry() == m_voltageEntry->entry) {
+ AddSample(run.voltage, record, voltageDouble, 1.0);
+ } else if (record.GetEntry() == m_positionEntry->entry) {
+ AddSample(run.position, record, positionDouble, m_positionScale);
+ } else if (record.GetEntry() == m_velocityEntry->entry) {
+ AddSample(run.velocity, record, velocityDouble, m_velocityScale);
+ }
+ }
+ }
+ }
+ }
+
+ return data;
+}
diff --git a/sysid/src/main/native/cpp/view/JSONConverter.cpp b/sysid/src/main/native/cpp/view/JSONConverter.cpp
deleted file mode 100644
index 88eaa6a..0000000
--- a/sysid/src/main/native/cpp/view/JSONConverter.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-// 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/LogLoader.cpp b/sysid/src/main/native/cpp/view/LogLoader.cpp
new file mode 100644
index 0000000..fdaa3af
--- /dev/null
+++ b/sysid/src/main/native/cpp/view/LogLoader.cpp
@@ -0,0 +1,208 @@
+// 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/LogLoader.h"
+
+#include <algorithm>
+#include <memory>
+#include <span>
+#include <string_view>
+
+#include <glass/support/DataLogReaderThread.h>
+#include <imgui.h>
+#include <imgui_stdlib.h>
+#include <portable-file-dialogs.h>
+#include <wpi/SpanExtras.h>
+#include <wpi/StringExtras.h>
+#include <wpi/fs.h>
+
+using namespace sysid;
+
+LogLoader::LogLoader(glass::Storage& storage, wpi::Logger& logger) {}
+
+LogLoader::~LogLoader() = default;
+
+void LogLoader::Display() {
+ if (ImGui::Button("Open data log file...")) {
+ m_opener = std::make_unique<pfd::open_file>(
+ "Select Data Log", "",
+ std::vector<std::string>{"DataLog Files", "*.wpilog"});
+ }
+
+ // Handle opening the file
+ if (m_opener && m_opener->ready(0)) {
+ if (!m_opener->result().empty()) {
+ m_filename = m_opener->result()[0];
+
+ std::error_code ec;
+ auto buf = wpi::MemoryBuffer::GetFile(m_filename, ec);
+ if (ec) {
+ ImGui::OpenPopup("Error");
+ m_error = fmt::format("Could not open file: {}", ec.message());
+ return;
+ }
+
+ wpi::log::DataLogReader reader{std::move(buf)};
+ if (!reader.IsValid()) {
+ ImGui::OpenPopup("Error");
+ m_error = "Not a valid datalog file";
+ return;
+ }
+ unload();
+ m_reader =
+ std::make_unique<glass::DataLogReaderThread>(std::move(reader));
+ m_entryTree.clear();
+ }
+ m_opener.reset();
+ }
+
+ // Handle errors
+ ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
+ if (ImGui::BeginPopupModal("Error")) {
+ ImGui::PushTextWrapPos(0.0f);
+ ImGui::TextUnformatted(m_error.c_str());
+ ImGui::PopTextWrapPos();
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ }
+ ImGui::EndPopup();
+ }
+
+ if (!m_reader) {
+ return;
+ }
+
+ // Summary info
+ ImGui::TextUnformatted(fs::path{m_filename}.stem().string().c_str());
+ ImGui::Text("%u records, %u entries%s", m_reader->GetNumRecords(),
+ m_reader->GetNumEntries(),
+ m_reader->IsDone() ? "" : " (working)");
+
+ if (!m_reader->IsDone()) {
+ return;
+ }
+
+ bool refilter = ImGui::InputText("Filter", &m_filter);
+
+ // Display tree of entries
+ if (m_entryTree.empty() || refilter) {
+ RebuildEntryTree();
+ }
+
+ ImGui::BeginTable(
+ "Entries", 2,
+ ImGuiTableFlags_Borders | ImGuiTableFlags_SizingStretchProp);
+ ImGui::TableSetupColumn("Name");
+ ImGui::TableSetupColumn("Type");
+ // ImGui::TableSetupColumn("Metadata");
+ ImGui::TableHeadersRow();
+ DisplayEntryTree(m_entryTree);
+ ImGui::EndTable();
+}
+
+void LogLoader::RebuildEntryTree() {
+ m_entryTree.clear();
+ wpi::SmallVector<std::string_view, 16> parts;
+ m_reader->ForEachEntryName([&](const glass::DataLogReaderEntry& entry) {
+ // only show double/float/string entries (TODO: support struct/protobuf)
+ if (entry.type != "double" && entry.type != "float" &&
+ entry.type != "string") {
+ return;
+ }
+
+ // filter on name
+ if (!m_filter.empty() && !wpi::contains_lower(entry.name, m_filter)) {
+ return;
+ }
+
+ parts.clear();
+ // split on first : if one is present
+ auto [prefix, mainpart] = wpi::split(entry.name, ':');
+ if (mainpart.empty() || wpi::contains(prefix, '/')) {
+ mainpart = entry.name;
+ } else {
+ parts.emplace_back(prefix);
+ }
+ wpi::split(mainpart, parts, '/', -1, false);
+
+ // ignore a raw "/" key
+ if (parts.empty()) {
+ return;
+ }
+
+ // get to leaf
+ auto nodes = &m_entryTree;
+ for (auto part : wpi::drop_back(std::span{parts.begin(), parts.end()})) {
+ auto it =
+ std::find_if(nodes->begin(), nodes->end(),
+ [&](const auto& node) { return node.name == part; });
+ if (it == nodes->end()) {
+ nodes->emplace_back(part);
+ // path is from the beginning of the string to the end of the current
+ // part; this works because part is a reference to the internals of
+ // entry.name
+ nodes->back().path.assign(
+ entry.name.data(), part.data() + part.size() - entry.name.data());
+ it = nodes->end() - 1;
+ }
+ nodes = &it->children;
+ }
+
+ auto it = std::find_if(nodes->begin(), nodes->end(), [&](const auto& node) {
+ return node.name == parts.back();
+ });
+ if (it == nodes->end()) {
+ nodes->emplace_back(parts.back());
+ // no need to set path, as it's identical to entry.name
+ it = nodes->end() - 1;
+ }
+ it->entry = &entry;
+ });
+}
+
+static void EmitEntry(const std::string& name,
+ const glass::DataLogReaderEntry& entry) {
+ ImGui::TableNextColumn();
+ ImGui::Selectable(name.c_str());
+ if (ImGui::BeginDragDropSource()) {
+ auto entryPtr = &entry;
+ ImGui::SetDragDropPayload(
+ entry.type == "string" ? "DataLogEntryString" : "DataLogEntry",
+ &entryPtr,
+ sizeof(entryPtr)); // NOLINT
+ ImGui::TextUnformatted(entry.name.data(),
+ entry.name.data() + entry.name.size());
+ ImGui::EndDragDropSource();
+ }
+ ImGui::TableNextColumn();
+ ImGui::TextUnformatted(entry.type.data(),
+ entry.type.data() + entry.type.size());
+#if 0
+ ImGui::TableNextColumn();
+ ImGui::TextUnformatted(entry.metadata.data(),
+ entry.metadata.data() + entry.metadata.size());
+#endif
+}
+
+void LogLoader::DisplayEntryTree(const std::vector<EntryTreeNode>& tree) {
+ for (auto&& node : tree) {
+ if (node.entry) {
+ EmitEntry(node.name, *node.entry);
+ }
+
+ if (!node.children.empty()) {
+ ImGui::TableNextColumn();
+ bool open = ImGui::TreeNodeEx(node.name.c_str(),
+ ImGuiTreeNodeFlags_SpanFullWidth);
+ ImGui::TableNextColumn();
+#if 0
+ ImGui::TableNextColumn();
+#endif
+ if (open) {
+ DisplayEntryTree(node.children);
+ ImGui::TreePop();
+ }
+ }
+ }
+}
diff --git a/sysid/src/main/native/cpp/view/Logger.cpp b/sysid/src/main/native/cpp/view/Logger.cpp
deleted file mode 100644
index 5e7773d..0000000
--- a/sysid/src/main/native/cpp/view/Logger.cpp
+++ /dev/null
@@ -1,222 +0,0 @@
-// 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
index 38cf8b2..1b8ede9 100644
--- a/sysid/src/main/native/include/sysid/Util.h
+++ b/sysid/src/main/native/include/sysid/Util.h
@@ -34,7 +34,7 @@
#define STRINGIZE(s) #s
namespace sysid {
-static constexpr const char* kUnits[] = {"Meters", "Feet", "Inches",
+inline constexpr const char* kUnits[] = {"Meters", "Feet", "Inches",
"Radians", "Rotations", "Degrees"};
/**
diff --git a/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
index d572578..d2c38e1 100644
--- a/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
+++ b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
@@ -17,6 +17,7 @@
#include <units/time.h>
#include <wpi/Logger.h>
+#include <wpi/StringMap.h>
#include <wpi/json.h>
#include "sysid/analysis/AnalysisType.h"
@@ -33,6 +34,9 @@
*/
class AnalysisManager {
public:
+ // This contains data for each test (e.g. quasistatic-forward,
+ // quasistatic-backward, etc) indexed by test name
+ TestData m_data;
/**
* Represents settings for an instance of the analysis manager. This contains
* information about the feedback controller preset, loop type, motion
@@ -40,7 +44,6 @@
* dataset.
*/
struct Settings {
- enum class DrivetrainDataset { kCombined = 0, kLeft = 1, kRight = 2 };
/**
* The feedback controller preset used to calculate gains.
*/
@@ -87,8 +90,6 @@
* in a smart motor controller).
*/
bool convertGainsToEncTicks = false;
-
- DrivetrainDataset dataset = DrivetrainDataset::kCombined;
};
/**
@@ -98,18 +99,14 @@
/**
* Stores the Feedforward gains.
*/
- std::tuple<std::vector<double>, double, double> ffGains;
-
- /**
- * Stores the trackwidth for angular drivetrain tests.
- */
- std::optional<double> trackWidth;
+ OLSResult ffGains;
};
/**
* Exception for File Reading Errors.
*/
- struct FileReadingError : public std::exception {
+ class FileReadingError : public std::exception {
+ public:
/**
* Creates a FileReadingError object
*
@@ -119,18 +116,21 @@
msg = fmt::format("Unable to read: {}", path);
}
+ const char* what() const noexcept override { return msg.c_str(); }
+
+ private:
/**
* 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"};
+ "quasistatic-forward", "quasistatic-reverse", "dynamic-forward",
+ "dynamic-reverse"};
/**
* Concatenates a list of vectors. The contents of the source vectors are
@@ -167,12 +167,11 @@
* 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 data The data from the SysId routine.
* @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);
+ AnalysisManager(TestData data, Settings& settings, wpi::Logger& logger);
/**
* Prepares data from the JSON and stores the output in Storage member
@@ -200,10 +199,8 @@
* 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);
+ void OverrideUnits(std::string_view unit);
/**
* Resets the units back to those defined in the JSON.
@@ -215,21 +212,14 @@
*
* @return The analysis type.
*/
- const AnalysisType& GetAnalysisType() const { return m_type; }
+ const AnalysisType& GetAnalysisType() const { return m_data.mechanismType; }
/**
* 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; }
+ std::string_view GetUnit() const { return m_data.distanceUnit; }
/**
* Returns a reference to the iterator of the currently selected raw datset.
@@ -238,9 +228,7 @@
*
* @return A reference to the raw internal data.
*/
- Storage& GetRawData() {
- return m_rawDataset[static_cast<int>(m_settings.dataset)];
- }
+ Storage& GetRawData() { return m_rawDataset; }
/**
* Returns a reference to the iterator of the currently selected filtered
@@ -249,18 +237,14 @@
*
* @return A reference to the filtered internal data.
*/
- Storage& GetFilteredData() {
- return m_filteredDataset[static_cast<int>(m_settings.dataset)];
- }
+ Storage& GetFilteredData() { return m_filteredDataset; }
/**
* Returns the original dataset.
*
* @return The original (untouched) dataset
*/
- Storage& GetOriginalData() {
- return m_originalDataset[static_cast<int>(m_settings.dataset)];
- }
+ Storage& GetOriginalData() { return m_originalDataset; }
/**
* Returns the minimum duration of the Step Voltage Test of the currently
@@ -311,22 +295,14 @@
return m_startTimes;
}
- bool HasData() const {
- return !m_originalDataset[static_cast<int>(
- Settings::DrivetrainDataset::kCombined)]
- .empty();
- }
+ bool HasData() const { return !m_originalDataset.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;
+ Storage m_originalDataset;
+ Storage m_rawDataset;
+ Storage m_filteredDataset;
// Stores the various start times of the different tests.
std::array<units::second_t, 4> m_startTimes;
@@ -335,24 +311,11 @@
// 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
index 7feedb3..5a30d7c 100644
--- a/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
+++ b/sysid/src/main/native/include/sysid/analysis/AnalysisType.h
@@ -52,11 +52,9 @@
};
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"};
+inline constexpr AnalysisType kElevator{4, 4, "Elevator"};
+inline constexpr AnalysisType kArm{5, 4, "Arm"};
+inline constexpr AnalysisType kSimple{3, 4, "Simple"};
AnalysisType FromName(std::string_view name);
} // namespace analysis
diff --git a/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h b/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
index 4b13c6c..d946c48 100644
--- a/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
+++ b/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h
@@ -71,11 +71,11 @@
enum class FeedbackControllerLoopType { kPosition, kVelocity };
namespace presets {
-constexpr FeedbackControllerPreset kDefault{1.0, 1.0, 20_ms, true, 0_s};
+inline 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};
+inline constexpr FeedbackControllerPreset kWPILibNew{kDefault};
+inline constexpr FeedbackControllerPreset kWPILibOld{1.0 / 12.0, 1.0, 50_ms,
+ false, 0_s};
// Measurement delay from a moving average filter:
//
@@ -117,10 +117,10 @@
*
* 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};
+inline constexpr FeedbackControllerPreset kCTRECANCoder{1.0 / 12.0, 60.0, 1_ms,
+ true, 81.5_ms};
+inline 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
*
@@ -129,7 +129,8 @@
* 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};
+inline constexpr FeedbackControllerPreset kCTREProDefault{1.0, 1.0, 1_ms, true,
+ 1_ms};
/**
* https://github.com/wpilibsuite/sysid/issues/258#issuecomment-1010658237
@@ -138,8 +139,8 @@
*
* 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};
+inline 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
@@ -150,15 +151,15 @@
*
* 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};
+inline 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};
+inline 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
index fc9e47c..d2503ce 100644
--- a/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
+++ b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
@@ -4,22 +4,50 @@
#pragma once
+#include <string>
#include <tuple>
#include <vector>
#include "sysid/analysis/AnalysisType.h"
+#include "sysid/analysis/OLS.h"
#include "sysid/analysis/Storage.h"
namespace sysid {
/**
+ * Exception for data that doesn't sample enough of the state-input space.
+ */
+class InsufficientSamplesError : public std::exception {
+ public:
+ /**
+ * Constructs an InsufficientSamplesError.
+ *
+ * @param message The error message
+ */
+ explicit InsufficientSamplesError(std::string_view message) {
+ m_message = message;
+ }
+
+ const char* what() const noexcept override { return m_message.c_str(); }
+
+ private:
+ /**
+ * Stores the error message
+ */
+ std::string m_message;
+};
+
+/**
* 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.
+ * @param data The OLS input data.
+ * @param type The analysis type.
+ * @param throwOnRankDeficiency Whether to throw if the fit is going to be poor.
+ * This option is provided for unit testing purposes.
*/
-std::tuple<std::vector<double>, double, double> CalculateFeedforwardGains(
- const Storage& data, const AnalysisType& type);
+OLSResult CalculateFeedforwardGains(const Storage& data,
+ const AnalysisType& type,
+ bool throwOnRankDeficiency = true);
+
} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
index 9030c00..28538a1 100644
--- a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
+++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
@@ -14,6 +14,7 @@
#include <utility>
#include <vector>
+#include <fmt/format.h>
#include <frc/filter/LinearFilter.h>
#include <units/time.h>
#include <wpi/StringMap.h>
@@ -30,7 +31,8 @@
* 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 {
+class InvalidDataError : public std::exception {
+ public:
/**
* Creates an InvalidDataError Exception. It adds additional steps after the
* initial error message to inform users in the ways that they could fix their
@@ -46,17 +48,20 @@
message);
}
+ const char* what() const noexcept override { return m_message.c_str(); }
+
+ private:
/**
* 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 {
+class NoQuasistaticDataError : public std::exception {
+ public:
const char* what() const noexcept override {
return "Quasistatic test trimming removed all data. Please adjust your "
"motion threshold and double check "
@@ -68,7 +73,8 @@
/**
* Exception for Dynamic Data being completely removed.
*/
-struct NoDynamicDataError : public std::exception {
+class NoDynamicDataError : public std::exception {
+ public:
const char* what() const noexcept override {
return "Dynamic test trimming removed all data. Please adjust your test "
"duration and double check "
@@ -92,6 +98,9 @@
const std::vector<PreparedData>& data, int window,
std::function<double(const PreparedData&)> accessorFunction);
+double GetMaxSpeed(const std::vector<PreparedData>& data,
+ std::function<double(const PreparedData&)> accessorFunction);
+
/**
* Reduces noise in velocity data by applying a median filter.
*
diff --git a/sysid/src/main/native/include/sysid/analysis/JSONConverter.h b/sysid/src/main/native/include/sysid/analysis/JSONConverter.h
deleted file mode 100644
index 7581d25..0000000
--- a/sysid/src/main/native/include/sysid/analysis/JSONConverter.h
+++ /dev/null
@@ -1,24 +0,0 @@
-// 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
index cf97904..43f447a 100644
--- a/sysid/src/main/native/include/sysid/analysis/OLS.h
+++ b/sysid/src/main/native/include/sysid/analysis/OLS.h
@@ -5,22 +5,29 @@
#pragma once
#include <cstddef>
-#include <tuple>
#include <vector>
#include <Eigen/Core>
namespace sysid {
+struct OLSResult {
+ /// Regression coeficients.
+ std::vector<double> coeffs;
+
+ /// R² (coefficient of determination)
+ double rSquared = 0.0;
+
+ /// Root-mean-square error
+ double rmse = 0.0;
+};
+
/**
- * 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.
+ * Performs ordinary least squares multiple regression on the provided data.
*
* @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);
+OLSResult OLS(const Eigen::MatrixXd& X, const Eigen::VectorXd& y);
} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/analysis/Storage.h b/sysid/src/main/native/include/sysid/analysis/Storage.h
index 52899a0..dad38e7 100644
--- a/sysid/src/main/native/include/sysid/analysis/Storage.h
+++ b/sysid/src/main/native/include/sysid/analysis/Storage.h
@@ -4,12 +4,46 @@
#pragma once
+#include <string>
#include <vector>
#include <units/time.h>
+#include <units/voltage.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/analysis/AnalysisType.h"
namespace sysid {
+struct MotorData {
+ // name of the *motor*, not the test
+ std::string name;
+
+ // Data for a single contiguous motor test
+ // Timestamps are not necessarily aligned!
+ struct Run {
+ template <typename T>
+ requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+ struct Sample {
+ Sample(units::second_t time, T measurement)
+ : time{time}, measurement{measurement} {}
+ units::second_t time;
+ T measurement;
+ };
+ std::vector<Sample<units::volt_t>> voltage;
+ std::vector<Sample<double>> position;
+ std::vector<Sample<double>> velocity;
+ };
+
+ std::vector<Run> runs;
+};
+
+struct TestData {
+ std::string distanceUnit;
+ AnalysisType mechanismType;
+ wpi::StringMap<MotorData> motorData;
+};
+
/**
* Represents each data point after it is cleaned and various parameters are
* calculated.
diff --git a/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h b/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
deleted file mode 100644
index 85ee09e..0000000
--- a/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
+++ /dev/null
@@ -1,237 +0,0 @@
-// 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
index 2f30f61..7dcfd41 100644
--- a/sysid/src/main/native/include/sysid/view/Analyzer.h
+++ b/sysid/src/main/native/include/sysid/view/Analyzer.h
@@ -40,11 +40,12 @@
*/
class Analyzer : public glass::View {
public:
+ TestData m_data;
/**
* The different display and processing states for the GUI
*/
enum class AnalyzerState {
- kWaitingForJSON,
+ kWaitingForData,
kNominalDisplay,
kMotionThresholdError,
kTestDurationError,
@@ -90,12 +91,12 @@
~Analyzer() override { AbortDataPrep(); };
- private:
/**
- * Handles the logic for selecting a json to analyze
+ * Analyzes the selected data.
*/
- void SelectFile();
+ void AnalyzeData();
+ private:
/**
* Kills the data preparation thread
*/
@@ -113,11 +114,6 @@
void DisplayGraphs();
/**
- * Displays the file selection widget.
- */
- void DisplayFileSelector();
-
- /**
* Resets the current analysis data.
*/
void ResetData();
@@ -196,7 +192,7 @@
void HandleError(std::string_view msg);
// State of the Display GUI
- AnalyzerState m_state = AnalyzerState::kWaitingForJSON;
+ AnalyzerState m_state = AnalyzerState::kWaitingForData;
// Stores the exception message.
std::string m_exception;
@@ -221,29 +217,21 @@
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;
+ float m_stepTestDuration = 10;
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;
diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h
new file mode 100644
index 0000000..95d7e19
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/DataSelector.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
+
+#include <functional>
+#include <future>
+#include <map>
+#include <string>
+#include <vector>
+
+#include <glass/View.h>
+#include <glass/support/DataLogReaderThread.h>
+#include <wpi/StringMap.h>
+
+#include "sysid/analysis/Storage.h"
+
+namespace glass {
+class DataLogReaderEntry;
+class Storage;
+} // namespace glass
+
+namespace wpi {
+class Logger;
+} // namespace wpi
+
+namespace sysid {
+/**
+ * Helps with loading datalog files.
+ */
+class DataSelector : public glass::View {
+ public:
+ /**
+ * Creates a data selector widget
+ *
+ * @param logger The program logger
+ */
+ explicit DataSelector(glass::Storage& storage, wpi::Logger& logger)
+ /*: m_logger{logger}*/ {}
+
+ /**
+ * Displays the log loader window.
+ */
+ void Display() override;
+
+ /**
+ * Resets view. Must be called whenever the DataLogReader goes away, as this
+ * class keeps references to DataLogReaderEntry objects.
+ */
+ void Reset();
+
+ /**
+ * Called when new test data is loaded.
+ */
+ std::function<void(TestData)> testdata;
+
+ private:
+ // wpi::Logger& m_logger;
+ using Runs = std::vector<glass::DataLogReaderRange>;
+ using State = std::map<std::string, Runs, std::less<>>; // full name
+ using Tests = std::map<std::string, State, std::less<>>; // e.g. "dynamic"
+ std::future<Tests> m_testsFuture;
+ Tests m_tests;
+ std::string m_selectedTest;
+ const glass::DataLogReaderEntry* m_testStateEntry = nullptr;
+ const glass::DataLogReaderEntry* m_velocityEntry = nullptr;
+ const glass::DataLogReaderEntry* m_positionEntry = nullptr;
+ const glass::DataLogReaderEntry* m_voltageEntry = nullptr;
+ double m_velocityScale = 1.0;
+ double m_positionScale = 1.0;
+ int m_selectedUnit = 0;
+ int m_selectedAnalysis = 0;
+ std::future<TestData> m_testdataFuture;
+ std::vector<std::string> m_testdataStats;
+
+ static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry);
+ TestData BuildTestData();
+};
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/JSONConverter.h b/sysid/src/main/native/include/sysid/view/JSONConverter.h
deleted file mode 100644
index 89bfa32..0000000
--- a/sysid/src/main/native/include/sysid/view/JSONConverter.h
+++ /dev/null
@@ -1,57 +0,0 @@
-// 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/LogLoader.h b/sysid/src/main/native/include/sysid/view/LogLoader.h
new file mode 100644
index 0000000..04ddd84
--- /dev/null
+++ b/sysid/src/main/native/include/sysid/view/LogLoader.h
@@ -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.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <glass/View.h>
+#include <wpi/Signal.h>
+
+namespace glass {
+class DataLogReaderEntry;
+class DataLogReaderThread;
+class Storage;
+} // namespace glass
+
+namespace pfd {
+class open_file;
+} // namespace pfd
+
+namespace wpi {
+class Logger;
+} // namespace wpi
+
+namespace sysid {
+/**
+ * Helps with loading datalog files.
+ */
+class LogLoader : public glass::View {
+ public:
+ /**
+ * Creates a log loader widget
+ *
+ * @param logger The program logger
+ */
+ explicit LogLoader(glass::Storage& storage, wpi::Logger& logger);
+
+ ~LogLoader() override;
+
+ /**
+ * Displays the log loader window.
+ */
+ void Display() override;
+
+ /**
+ * Signal called when the current file is unloaded (invalidates any
+ * LogEntry*).
+ */
+ wpi::sig::Signal<> unload;
+
+ private:
+ // wpi::Logger& m_logger;
+
+ std::string m_filename;
+ std::unique_ptr<pfd::open_file> m_opener;
+ std::unique_ptr<glass::DataLogReaderThread> m_reader;
+
+ std::string m_error;
+
+ std::string m_filter;
+
+ struct EntryTreeNode {
+ explicit EntryTreeNode(std::string_view name) : name{name} {}
+ std::string name; // name of just this node
+ std::string path; // full path if entry is nullptr
+ const glass::DataLogReaderEntry* entry = nullptr;
+ std::vector<EntryTreeNode> children; // children, sorted by name
+ };
+ std::vector<EntryTreeNode> m_entryTree;
+
+ void RebuildEntryTree();
+ void DisplayEntryTree(const std::vector<EntryTreeNode>& nodes);
+};
+} // namespace sysid
diff --git a/sysid/src/main/native/include/sysid/view/Logger.h b/sysid/src/main/native/include/sysid/view/Logger.h
deleted file mode 100644
index d06d650..0000000
--- a/sysid/src/main/native/include/sysid/view/Logger.h
+++ /dev/null
@@ -1,82 +0,0 @@
-// 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
index 732a1aa..f5d1e44 100644
--- a/sysid/src/main/native/include/sysid/view/UILayout.h
+++ b/sysid/src/main/native/include/sysid/view/UILayout.h
@@ -62,9 +62,12 @@
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};
+inline constexpr Vector2d kLogLoaderWindowPos = kLeftColPos;
+inline constexpr Vector2d kLogLoaderWindowSize{kLeftColSize.x, 450};
+inline constexpr Vector2d kDataSelectorWindowPos =
+ kLogLoaderWindowPos + Vector2d{0, kLogLoaderWindowSize.y + kWindowGap};
+inline constexpr Vector2d kDataSelectorWindowSize{
+ kLeftColSize.x, kAppWindowSize.y - kWindowGap - kDataSelectorWindowPos.y};
// Center column position and size
inline constexpr Vector2d kCenterColPos =
diff --git a/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp b/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
index 0abb2a1..51d348c 100644
--- a/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
+++ b/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
@@ -7,10 +7,6 @@
#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"));
diff --git a/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
index a52840d..d8cd79e 100644
--- a/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
+++ b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
@@ -2,28 +2,43 @@
// 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 <stdint.h>
+
+#include <bitset>
#include <cmath>
+#include <span>
#include <gtest/gtest.h>
#include <units/time.h>
#include <units/voltage.h>
#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
#include "sysid/analysis/ArmSim.h"
#include "sysid/analysis/ElevatorSim.h"
#include "sysid/analysis/FeedforwardAnalysis.h"
#include "sysid/analysis/SimpleMotorSim.h"
+namespace {
+
+enum Movements : uint32_t {
+ kSlowForward,
+ kSlowBackward,
+ kFastForward,
+ kFastBackward
+};
+
+inline constexpr int kMovementCombinations = 16;
+
/**
* 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.
+ * @tparam Model The model type.
+ * @param model The simulation model.
+ * @param movements Which movements to do.
*/
template <typename Model>
-sysid::Storage CollectData(Model& model) {
+sysid::Storage CollectData(Model& model, std::bitset<4> movements) {
constexpr auto kUstep = 0.25_V / 1_s;
constexpr units::volt_t kUmax = 7_V;
constexpr units::second_t T = 5_ms;
@@ -31,221 +46,247 @@
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 forward
+ if (movements.test(Movements::kSlowForward)) {
+ model.Reset();
+ 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())});
+ // Slow backward
+ if (movements.test(Movements::kSlowBackward)) {
+ 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;
+ 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())});
+ // Fast forward
+ if (movements.test(Movements::kFastForward)) {
+ 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;
+ 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())});
+ // Fast backward
+ if (movements.test(Movements::kFastBackward)) {
+ 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;
+ 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;
+/**
+ * Asserts success if the gains contain NaNs or are too far from their expected
+ * values.
+ *
+ * @param expectedGains The expected feedforward gains.
+ * @param actualGains The calculated feedforward gains.
+ * @param tolerances The tolerances for the coefficient comparisons.
+ */
+testing::AssertionResult FitIsBad(std::span<const double> expectedGains,
+ std::span<const double> actualGains,
+ std::span<const double> tolerances) {
+ // Check for NaN
+ for (const auto& coeff : actualGains) {
+ if (std::isnan(coeff)) {
+ return testing::AssertionSuccess();
+ }
+ }
- 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);
+ for (size_t i = 0; i < expectedGains.size(); ++i) {
+ if (std::abs(expectedGains[i] - actualGains[i]) >= tolerances[i]) {
+ return testing::AssertionSuccess();
+ }
+ }
- 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);
+ auto result = testing::AssertionFailure();
+
+ result << "\n";
+ for (size_t i = 0; i < expectedGains.size(); ++i) {
+ if (i == 0) {
+ result << "Ks";
+ } else if (i == 1) {
+ result << "Kv";
+ } else if (i == 2) {
+ result << "Ka";
+ } else if (i == 3) {
+ result << "Kg";
+ } else if (i == 4) {
+ result << "offset";
+ }
+
+ result << ":\n";
+ result << " expected " << expectedGains[i] << ",\n";
+ result << " actual " << actualGains[i] << ",\n";
+ result << " diff " << std::abs(expectedGains[i] - actualGains[i]) << "\n";
+ }
+
+ return result;
+}
+
+/**
+ * Asserts that two arrays are equal.
+ *
+ * @param expected The expected array.
+ * @param actual The actual array.
+ * @param tolerances The tolerances for the element comparisons.
+ */
+void ExpectArrayNear(std::span<const double> expected,
+ std::span<const double> actual,
+ std::span<const double> tolerances) {
+ // Check size
+ const size_t size = expected.size();
+ EXPECT_EQ(size, actual.size());
+ EXPECT_EQ(size, tolerances.size());
+
+ // Check elements
+ for (size_t i = 0; i < size; ++i) {
+ EXPECT_NEAR(expected[i], actual[i], tolerances[i]) << "where i = " << i;
}
}
-TEST(FeedforwardAnalysisTest, Arm2) {
- constexpr double Ks = 0.547;
- constexpr double Kv = 0.0693;
- constexpr double Ka = 0.1170;
- constexpr double Kg = 0.122;
+/**
+ * @tparam Model The model type.
+ * @param model The simulation model.
+ * @param type The analysis type.
+ * @param expectedGains The expected feedforward gains.
+ * @param tolerances The tolerances for the coefficient comparisons.
+ */
+template <typename Model>
+void RunTests(Model& model, const sysid::AnalysisType& type,
+ std::span<const double> expectedGains,
+ std::span<const double> tolerances) {
+ // Iterate through all combinations of movements
+ for (int movements = 0; movements < kMovementCombinations; ++movements) {
+ try {
+ auto ff =
+ sysid::CalculateFeedforwardGains(CollectData(model, movements), type);
- 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);
+ ExpectArrayNear(expectedGains, ff.coeffs, tolerances);
+ } catch (sysid::InsufficientSamplesError&) {
+ // If calculation threw an exception, confirm at least one of the gains
+ // doesn't match
+ auto ff = sysid::CalculateFeedforwardGains(CollectData(model, movements),
+ type, false);
+ EXPECT_TRUE(FitIsBad(expectedGains, ff.coeffs, tolerances));
+ }
}
}
-TEST(FeedforwardAnalysisTest, Drivetrain1) {
- constexpr double Ks = 1.01;
- constexpr double Kv = 3.060;
- constexpr double Ka = 0.327;
+} // namespace
- sysid::SimpleMotorSim model{Ks, Kv, Ka};
- auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
- sysid::analysis::kDrivetrain);
- auto& gains = std::get<0>(ff);
+TEST(FeedforwardAnalysisTest, Arm) {
+ {
+ constexpr double Ks = 1.01;
+ constexpr double Kv = 3.060;
+ constexpr double Ka = 0.327;
+ constexpr double Kg = 0.211;
- EXPECT_NEAR(gains[0], Ks, 0.003);
- EXPECT_NEAR(gains[1], Kv, 0.003);
- EXPECT_NEAR(gains[2], Ka, 0.003);
+ for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) {
+ sysid::ArmSim model{Ks, Kv, Ka, Kg, offset};
+
+ RunTests(model, sysid::analysis::kArm, {{Ks, Kv, Ka, Kg, offset}},
+ {{8e-3, 8e-3, 8e-3, 8e-3, 3e-2}});
+ }
+ }
+
+ {
+ 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};
+
+ RunTests(model, sysid::analysis::kArm, {{Ks, Kv, Ka, Kg, offset}},
+ {{8e-3, 8e-3, 8e-3, 8e-3, 5e-2}});
+ }
+ }
}
-TEST(FeedforwardAnalysisTest, Drivetrain2) {
- constexpr double Ks = 0.547;
- constexpr double Kv = 0.0693;
- constexpr double Ka = 0.1170;
+TEST(FeedforwardAnalysisTest, Elevator) {
+ {
+ constexpr double Ks = 1.01;
+ constexpr double Kv = 3.060;
+ constexpr double Ka = 0.327;
+ constexpr double Kg = -0.211;
- sysid::SimpleMotorSim model{Ks, Kv, Ka};
- auto ff = sysid::CalculateFeedforwardGains(CollectData(model),
- sysid::analysis::kDrivetrain);
- auto& gains = std::get<0>(ff);
+ sysid::ElevatorSim model{Ks, Kv, Ka, Kg};
- EXPECT_NEAR(gains[0], Ks, 0.003);
- EXPECT_NEAR(gains[1], Kv, 0.003);
- EXPECT_NEAR(gains[2], Ka, 0.003);
+ RunTests(model, sysid::analysis::kElevator, {{Ks, Kv, Ka, Kg}},
+ {{8e-3, 8e-3, 8e-3, 8e-3}});
+ }
+
+ {
+ 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};
+
+ RunTests(model, sysid::analysis::kElevator, {{Ks, Kv, Ka, Kg}},
+ {{8e-3, 8e-3, 8e-3, 8e-3}});
+ }
}
-TEST(FeedforwardAnalysisTest, DrivetrainAngular1) {
- constexpr double Ks = 1.01;
- constexpr double Kv = 3.060;
- constexpr double Ka = 0.327;
+TEST(FeedforwardAnalysisTest, Simple) {
+ {
+ 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);
+ sysid::SimpleMotorSim model{Ks, Kv, Ka};
- EXPECT_NEAR(gains[0], Ks, 0.003);
- EXPECT_NEAR(gains[1], Kv, 0.003);
- EXPECT_NEAR(gains[2], Ka, 0.003);
-}
+ RunTests(model, sysid::analysis::kSimple, {{Ks, Kv, Ka}},
+ {{8e-3, 8e-3, 8e-3}});
+ }
-TEST(FeedforwardAnalysisTest, DrivetrainAngular2) {
- constexpr double Ks = 0.547;
- constexpr double Kv = 0.0693;
- constexpr double Ka = 0.1170;
+ {
+ 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);
+ sysid::SimpleMotorSim model{Ks, Kv, Ka};
- 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);
+ RunTests(model, sysid::analysis::kSimple, {{Ks, Kv, Ka}},
+ {{8e-3, 8e-3, 8e-3}});
+ }
}
diff --git a/sysid/src/test/native/cpp/analysis/FilterTest.cpp b/sysid/src/test/native/cpp/analysis/FilterTest.cpp
index a7b0349..8cfebe4 100644
--- a/sysid/src/test/native/cpp/analysis/FilterTest.cpp
+++ b/sysid/src/test/native/cpp/analysis/FilterTest.cpp
@@ -45,19 +45,13 @@
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},
+ {0_s, 1, 2, 0, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 0.25, 0},
+ {2_s, 1, 2, 0, 5_ms, 10, 0}, {3_s, 1, 2, 3, 5_ms, 0.45, 0},
+ {4_s, 1, 2, 9.6, 5_ms, 0, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
+ {6_s, 1, 2, 0, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0},
+ {8_s, 1, 2, 10, 5_ms, 0, 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;
@@ -66,9 +60,7 @@
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(4, settings.stepTestDuration.value());
EXPECT_EQ(2, minTime.value());
}
diff --git a/sysid/src/test/native/cpp/analysis/OLSTest.cpp b/sysid/src/test/native/cpp/analysis/OLSTest.cpp
index bf20516..558b7e3 100644
--- a/sysid/src/test/native/cpp/analysis/OLSTest.cpp
+++ b/sysid/src/test/native/cpp/analysis/OLSTest.cpp
@@ -11,12 +11,12 @@
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);
+ auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
+ EXPECT_EQ(coeffs.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);
+ EXPECT_NEAR(coeffs[0], 1.0, 0.05);
+ EXPECT_NEAR(coeffs[1], 2.0, 0.05);
+ EXPECT_NEAR(rSquared, 1.0, 1e-4);
}
TEST(OLSTest, TwoVariablesFivePoints) {
@@ -25,12 +25,12 @@
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);
+ auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
+ EXPECT_EQ(coeffs.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);
+ EXPECT_NEAR(coeffs[0], 0.305, 0.05);
+ EXPECT_NEAR(coeffs[1], 1.518, 0.05);
+ EXPECT_NEAR(rSquared, 0.985, 0.05);
}
#ifndef NDEBUG