From 9b79ef782ec7ddbaf9777ce1e8ae2b8b88b5eeba Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Tue, 28 Jan 2025 17:28:09 -0500 Subject: [PATCH 01/14] add exception --- sysid/src/main/native/cpp/view/Analyzer.cpp | 11 ++++++++++ .../include/sysid/analysis/FilteringUtils.h | 22 +++++++++++++++++++ .../main/native/include/sysid/view/Analyzer.h | 1 + .../native/include/sysid/view/DataSelector.h | 2 ++ 4 files changed, 36 insertions(+) diff --git a/sysid/src/main/native/cpp/view/Analyzer.cpp b/sysid/src/main/native/cpp/view/Analyzer.cpp index 03b39171b23..384a5ba18fe 100644 --- a/sysid/src/main/native/cpp/view/Analyzer.cpp +++ b/sysid/src/main/native/cpp/view/Analyzer.cpp @@ -24,6 +24,7 @@ #include "sysid/analysis/AnalysisType.h" #include "sysid/analysis/FeedbackControllerPreset.h" #include "sysid/analysis/FilteringUtils.h" +#include "sysid/view/DataSelector.h" #include "sysid/view/UILayout.h" using namespace sysid; @@ -57,6 +58,9 @@ void Analyzer::UpdateFeedforwardGains() { ? units::math::max(0_s, m_manager->GetPositionDelay()) : units::math::max(0_s, m_manager->GetVelocityDelay()); PrepareGraphs(); + } catch (const sysid::MissingTestsError& e) { + m_state = AnalyzerState::kMissingTestsError; + HandleError(e.what()); } catch (const sysid::InvalidDataError& e) { m_state = AnalyzerState::kGeneralDataError; HandleError(e.what()); @@ -251,6 +255,13 @@ void Analyzer::Display() { } break; } + case AnalyzerState::kMissingTestsError: { + CreateErrorPopup(m_errorPopup, m_exception); + if (!m_errorPopup) { + m_state = AnalyzerState::kWaitingForData; + } + break; + } case AnalyzerState::kGeneralDataError: case AnalyzerState::kTestDurationError: case AnalyzerState::kVelocityThresholdError: { diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h index 67266ae647b..dd27ebdc8f6 100644 --- a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h +++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h @@ -18,6 +18,7 @@ #include #include +#include "fmt/ranges.h" #include "sysid/analysis/AnalysisManager.h" #include "sysid/analysis/Storage.h" @@ -68,6 +69,27 @@ class NoQuasistaticDataError : public std::exception { } }; +/** + * Exception for not all tests being present. + */ +class MissingTestsError : public std::exception { + public: + explicit MissingTestsError(std::vector MissingTests) + : missingTests(std::move(MissingTests)) { + errorString = fmt::format( + "The following tests were not detected: {}. Make sure to perform all " + "four tests as described in the SysId documentation.", + fmt::join(missingTests, ", ")); + } + const char* what() const noexcept override { + return errorString.c_str(); + } + + private: + std::vector missingTests; + std::string errorString; +}; + /** * Exception for Dynamic Data being completely removed. */ diff --git a/sysid/src/main/native/include/sysid/view/Analyzer.h b/sysid/src/main/native/include/sysid/view/Analyzer.h index bb7763f26f8..e8809936c44 100644 --- a/sysid/src/main/native/include/sysid/view/Analyzer.h +++ b/sysid/src/main/native/include/sysid/view/Analyzer.h @@ -46,6 +46,7 @@ class Analyzer : public glass::View { kVelocityThresholdError, kTestDurationError, kGeneralDataError, + kMissingTestsError, kFileError }; /** diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h index 71732a7ed74..bd07c971173 100644 --- a/sysid/src/main/native/include/sysid/view/DataSelector.h +++ b/sysid/src/main/native/include/sysid/view/DataSelector.h @@ -74,8 +74,10 @@ class DataSelector : public glass::View { int m_selectedAnalysis = 0; std::future m_testdataFuture; std::vector m_testdataStats; + std::vector m_missingTests; static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry); TestData BuildTestData(); + //std::vector GetMissingTests(glass::DataLogReaderEntry testStateEntry); }; } // namespace sysid From c5b4a63dc2a15444307200b408a11e4e01f3de61 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Tue, 28 Jan 2025 22:59:59 -0500 Subject: [PATCH 02/14] detect missing tests --- sysid/src/main/native/cpp/view/DataSelector.cpp | 11 +++++++++++ .../native/include/sysid/analysis/FilteringUtils.h | 1 - .../src/main/native/include/sysid/view/DataSelector.h | 3 ++- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/sysid/src/main/native/cpp/view/DataSelector.cpp b/sysid/src/main/native/cpp/view/DataSelector.cpp index 3409bf6ee37..2e90c808172 100644 --- a/sysid/src/main/native/cpp/view/DataSelector.cpp +++ b/sysid/src/main/native/cpp/view/DataSelector.cpp @@ -5,7 +5,9 @@ #include "sysid/view/DataSelector.h" #include +#include #include +#include #include #include @@ -18,6 +20,7 @@ #include "sysid/Util.h" #include "sysid/analysis/AnalysisType.h" +#include "sysid/analysis/FilteringUtils.h" #include "sysid/analysis/Storage.h" using namespace sysid; @@ -111,6 +114,7 @@ void DataSelector::Display() { continue; } WPI_INFO(m_logger, "Loaded test state {}", it2->first); + executedTests.push_back(it2->first); ++it2; } if (it->second.empty()) { @@ -132,6 +136,13 @@ void DataSelector::Display() { return; } + if (executedTests.size() < 4) { + std::ranges::sort(executedTests); + std::ranges::set_difference(executedTests, kValidTests, + std::back_inserter(m_missingTests)); + throw sysid::MissingTestsError{m_missingTests}; + } + #if 0 // Test filtering if (ImGui::BeginCombo("Test", m_selectedTest.c_str())) { diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h index dd27ebdc8f6..7f233311874 100644 --- a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h +++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h @@ -4,7 +4,6 @@ #pragma once -#include #include #include #include diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h index bd07c971173..b6ca944ab0b 100644 --- a/sysid/src/main/native/include/sysid/view/DataSelector.h +++ b/sysid/src/main/native/include/sysid/view/DataSelector.h @@ -75,9 +75,10 @@ class DataSelector : public glass::View { std::future m_testdataFuture; std::vector m_testdataStats; std::vector m_missingTests; + std::vector kValidTests; static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry); TestData BuildTestData(); - //std::vector GetMissingTests(glass::DataLogReaderEntry testStateEntry); + std::vector executedTests{}; }; } // namespace sysid From b11572a69c71956cbf4902af1f598a5f3e748dcb Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 29 Jan 2025 13:25:49 -0500 Subject: [PATCH 03/14] throw in analyzer --- sysid/src/main/native/cpp/App.cpp | 4 +++- sysid/src/main/native/cpp/view/Analyzer.cpp | 9 ++++++--- sysid/src/main/native/cpp/view/DataSelector.cpp | 1 - sysid/src/main/native/include/sysid/view/Analyzer.h | 7 +++++++ sysid/src/main/native/include/sysid/view/DataSelector.h | 2 +- 5 files changed, 17 insertions(+), 6 deletions(-) diff --git a/sysid/src/main/native/cpp/App.cpp b/sysid/src/main/native/cpp/App.cpp index 6d1b9285a32..178a0f726ca 100644 --- a/sysid/src/main/native/cpp/App.cpp +++ b/sysid/src/main/native/cpp/App.cpp @@ -106,8 +106,10 @@ void Application(std::string_view saveDir) { auto analyzer = std::make_unique(storage, gLogger); logLoader->unload.connect([ds = dataSelector.get()] { ds->Reset(); }); - dataSelector->testdata = [_analyzer = analyzer.get()](auto data) { + dataSelector->testdata = [_analyzer = analyzer.get(), + ds = dataSelector.get()](auto data) { _analyzer->m_data = data; + _analyzer->SetMissingTests(ds->m_missingTests); _analyzer->AnalyzeData(); }; diff --git a/sysid/src/main/native/cpp/view/Analyzer.cpp b/sysid/src/main/native/cpp/view/Analyzer.cpp index 384a5ba18fe..7cc9590ac03 100644 --- a/sysid/src/main/native/cpp/view/Analyzer.cpp +++ b/sysid/src/main/native/cpp/view/Analyzer.cpp @@ -58,9 +58,6 @@ void Analyzer::UpdateFeedforwardGains() { ? units::math::max(0_s, m_manager->GetPositionDelay()) : units::math::max(0_s, m_manager->GetVelocityDelay()); PrepareGraphs(); - } catch (const sysid::MissingTestsError& e) { - m_state = AnalyzerState::kMissingTestsError; - HandleError(e.what()); } catch (const sysid::InvalidDataError& e) { m_state = AnalyzerState::kGeneralDataError; HandleError(e.what()); @@ -280,6 +277,9 @@ void Analyzer::Display() { void Analyzer::PrepareData() { WPI_INFO(m_logger, "{}", "Preparing data"); try { + if (m_missingTests.size() > 0) { + throw sysid::MissingTestsError{m_missingTests}; + } m_manager->PrepareData(); UpdateFeedforwardGains(); UpdateFeedbackGains(); @@ -292,6 +292,9 @@ void Analyzer::PrepareData() { } catch (const sysid::NoDynamicDataError& e) { m_state = AnalyzerState::kTestDurationError; HandleError(e.what()); + } catch (const sysid::MissingTestsError& e) { + m_state = AnalyzerState::kMissingTestsError; + HandleError(e.what()); } catch (const AnalysisManager::FileReadingError& e) { m_state = AnalyzerState::kFileError; HandleError(e.what()); diff --git a/sysid/src/main/native/cpp/view/DataSelector.cpp b/sysid/src/main/native/cpp/view/DataSelector.cpp index 2e90c808172..6c1c7c0c094 100644 --- a/sysid/src/main/native/cpp/view/DataSelector.cpp +++ b/sysid/src/main/native/cpp/view/DataSelector.cpp @@ -140,7 +140,6 @@ void DataSelector::Display() { std::ranges::sort(executedTests); std::ranges::set_difference(executedTests, kValidTests, std::back_inserter(m_missingTests)); - throw sysid::MissingTestsError{m_missingTests}; } #if 0 diff --git a/sysid/src/main/native/include/sysid/view/Analyzer.h b/sysid/src/main/native/include/sysid/view/Analyzer.h index e8809936c44..9a7cb4eba2b 100644 --- a/sysid/src/main/native/include/sysid/view/Analyzer.h +++ b/sysid/src/main/native/include/sysid/view/Analyzer.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -92,6 +93,11 @@ class Analyzer : public glass::View { */ void AnalyzeData(); + /** + * Used by DataSelector to import any missing tests. + */ + void SetMissingTests(std::vector); + private: /** * Kills the data preparation thread @@ -200,6 +206,7 @@ class Analyzer : public glass::View { // Stores the exception message. std::string m_exception; + std::vector m_missingTests; bool m_calcDefaults = false; diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h index b6ca944ab0b..ad51402466a 100644 --- a/sysid/src/main/native/include/sysid/view/DataSelector.h +++ b/sysid/src/main/native/include/sysid/view/DataSelector.h @@ -55,6 +55,7 @@ class DataSelector : public glass::View { * Called when new test data is loaded. */ std::function testdata; + std::vector m_missingTests; private: wpi::Logger& m_logger; @@ -74,7 +75,6 @@ class DataSelector : public glass::View { int m_selectedAnalysis = 0; std::future m_testdataFuture; std::vector m_testdataStats; - std::vector m_missingTests; std::vector kValidTests; static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry); From cc4af5bbcdc42cda1f7f98f2a66603e4e6f0b9af Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 29 Jan 2025 13:47:46 -0500 Subject: [PATCH 04/14] define tests setter --- sysid/src/main/native/cpp/view/Analyzer.cpp | 6 +++++- sysid/src/main/native/cpp/view/DataSelector.cpp | 1 - 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/sysid/src/main/native/cpp/view/Analyzer.cpp b/sysid/src/main/native/cpp/view/Analyzer.cpp index 7cc9590ac03..9264eac79fe 100644 --- a/sysid/src/main/native/cpp/view/Analyzer.cpp +++ b/sysid/src/main/native/cpp/view/Analyzer.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -24,7 +25,6 @@ #include "sysid/analysis/AnalysisType.h" #include "sysid/analysis/FeedbackControllerPreset.h" #include "sysid/analysis/FilteringUtils.h" -#include "sysid/view/DataSelector.h" #include "sysid/view/UILayout.h" using namespace sysid; @@ -338,6 +338,10 @@ void Analyzer::HandleError(std::string_view msg) { PrepareRawGraphs(); } +void Analyzer::SetMissingTests(std::vector missingTests) { + m_missingTests = missingTests; +} + void Analyzer::DisplayGraphs() { ImGui::SetNextWindowPos(ImVec2{kDiagnosticPlotWindowPos}, ImGuiCond_FirstUseEver); diff --git a/sysid/src/main/native/cpp/view/DataSelector.cpp b/sysid/src/main/native/cpp/view/DataSelector.cpp index 6c1c7c0c094..915d44bb775 100644 --- a/sysid/src/main/native/cpp/view/DataSelector.cpp +++ b/sysid/src/main/native/cpp/view/DataSelector.cpp @@ -20,7 +20,6 @@ #include "sysid/Util.h" #include "sysid/analysis/AnalysisType.h" -#include "sysid/analysis/FilteringUtils.h" #include "sysid/analysis/Storage.h" using namespace sysid; From edf0d7c35974a627a0a53cf086708d98a04e8a08 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 09:59:50 -0500 Subject: [PATCH 05/14] change to std::map --- sysid/src/main/native/cpp/view/DataSelector.cpp | 15 ++++++++++++--- .../main/native/include/sysid/view/DataSelector.h | 15 +++++++++++++-- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/sysid/src/main/native/cpp/view/DataSelector.cpp b/sysid/src/main/native/cpp/view/DataSelector.cpp index 915d44bb775..f87cc0304ae 100644 --- a/sysid/src/main/native/cpp/view/DataSelector.cpp +++ b/sysid/src/main/native/cpp/view/DataSelector.cpp @@ -113,7 +113,7 @@ void DataSelector::Display() { continue; } WPI_INFO(m_logger, "Loaded test state {}", it2->first); - executedTests.push_back(it2->first); + executedTests[it2->first] = true; ++it2; } if (it->second.empty()) { @@ -134,10 +134,19 @@ void DataSelector::Display() { } return; } + if (executedTests.size() < 4) { - std::ranges::sort(executedTests); - std::ranges::set_difference(executedTests, kValidTests, + std::vector executedKeys{}, validKeys{}; + + for (auto test : kValidTests) { + validKeys.push_back(test.first); + } + for (auto test : executedTests) { + executedKeys.push_back(test.first); + } + + std::ranges::set_difference(executedKeys, validKeys, std::back_inserter(m_missingTests)); } diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h index ad51402466a..3df8a6f7012 100644 --- a/sysid/src/main/native/include/sysid/view/DataSelector.h +++ b/sysid/src/main/native/include/sysid/view/DataSelector.h @@ -75,10 +75,21 @@ class DataSelector : public glass::View { int m_selectedAnalysis = 0; std::future m_testdataFuture; std::vector m_testdataStats; - std::vector kValidTests; + std::map> kValidTests = { + {"quasistatic-forward", true}, + {"quasistatic-reverse", true}, + {"dynamic-forward", true}, + {"dynamic-reverse", true} + }; + std::map> executedTests; //= { + // {"quasistatic-forward", false}, + // {"quasistatic-reverse", false}, + // {"dynamic-forward", false}, + // {"dynamic-reverse", false} + // }; static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry); TestData BuildTestData(); - std::vector executedTests{}; + }; } // namespace sysid From 5e97940f3460141414e1b1c319041c76cecd9676 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 13:01:47 -0500 Subject: [PATCH 06/14] alignas fail --- .../native/cpp/analysis/FeedbackAnalysis.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 7821812bd4e..815740df58e 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -52,9 +52,14 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( auto system = frc::LinearSystem<1, 1, 1>( Eigen::Matrix{0.0}, Eigen::Matrix{1.0}, Eigen::Matrix{1.0}, Eigen::Matrix{0.0}); + +alignas(double) + auto Q = frc::MakeCostMatrix(params.qv); + alignas(double) + auto R = frc::MakeCostMatrix(params.r); // Create an LQR with one state -- position. - frc::LinearQuadraticRegulator<1, 1> controller{ - system, {params.qp}, {params.r}, preset.period}; + frc::LinearQuadraticRegulator<1, 1> controller{system.A(), system.B(), Q, R, + preset.period}; // Compensate for any latency from sensor measurements, filtering, etc. controller.LatencyCompensate(system, preset.period, preset.measurementDelay); @@ -79,9 +84,13 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains( frc::LinearSystem<1, 1, 1> system{ frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka}, frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}}; + alignas(double) + auto Q = frc::MakeCostMatrix(params.qv); + alignas(double) + auto R = frc::MakeCostMatrix(params.r); // Create an LQR controller with 1 state -- velocity. - frc::LinearQuadraticRegulator<1, 1> controller{ - system, {params.qv}, {params.r}, preset.period}; + frc::LinearQuadraticRegulator<1, 1> controller{system.A(), system.B(), Q, R, + preset.period}; // Compensate for any latency from sensor measurements, filtering, etc. controller.LatencyCompensate(system, preset.period, preset.measurementDelay); From ce5d364703220ebce22142f502dd1f5747d88a62 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 16:49:53 -0500 Subject: [PATCH 07/14] make missingTests parameter const& --- sysid/src/main/native/include/sysid/view/Analyzer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sysid/src/main/native/include/sysid/view/Analyzer.h b/sysid/src/main/native/include/sysid/view/Analyzer.h index 9a7cb4eba2b..4b6e258341e 100644 --- a/sysid/src/main/native/include/sysid/view/Analyzer.h +++ b/sysid/src/main/native/include/sysid/view/Analyzer.h @@ -96,7 +96,7 @@ class Analyzer : public glass::View { /** * Used by DataSelector to import any missing tests. */ - void SetMissingTests(std::vector); + void SetMissingTests(const std::vector& missingTests); private: /** From af8a8c0d3d0e6eb17f70864bd0f9bb38bbe143c9 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 16:51:06 -0500 Subject: [PATCH 08/14] const& impl --- sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp | 2 +- sysid/src/main/native/cpp/view/Analyzer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 815740df58e..985c597bd8c 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -53,7 +53,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( Eigen::Matrix{0.0}, Eigen::Matrix{1.0}, Eigen::Matrix{1.0}, Eigen::Matrix{0.0}); -alignas(double) + alignas(double) auto Q = frc::MakeCostMatrix(params.qv); alignas(double) auto R = frc::MakeCostMatrix(params.r); diff --git a/sysid/src/main/native/cpp/view/Analyzer.cpp b/sysid/src/main/native/cpp/view/Analyzer.cpp index 9264eac79fe..726042586d9 100644 --- a/sysid/src/main/native/cpp/view/Analyzer.cpp +++ b/sysid/src/main/native/cpp/view/Analyzer.cpp @@ -338,7 +338,7 @@ void Analyzer::HandleError(std::string_view msg) { PrepareRawGraphs(); } -void Analyzer::SetMissingTests(std::vector missingTests) { +void Analyzer::SetMissingTests(const std::vector& missingTests) { m_missingTests = missingTests; } From 419cb865532e7ca62488207492d382615d36e176 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 22:23:21 -0500 Subject: [PATCH 09/14] use set and naive comparison --- .../src/main/native/cpp/view/DataSelector.cpp | 19 ++++++------------- .../native/include/sysid/view/DataSelector.h | 19 ++++++------------- 2 files changed, 12 insertions(+), 26 deletions(-) diff --git a/sysid/src/main/native/cpp/view/DataSelector.cpp b/sysid/src/main/native/cpp/view/DataSelector.cpp index f87cc0304ae..51d53c78eec 100644 --- a/sysid/src/main/native/cpp/view/DataSelector.cpp +++ b/sysid/src/main/native/cpp/view/DataSelector.cpp @@ -5,7 +5,6 @@ #include "sysid/view/DataSelector.h" #include -#include #include #include #include @@ -113,7 +112,7 @@ void DataSelector::Display() { continue; } WPI_INFO(m_logger, "Loaded test state {}", it2->first); - executedTests[it2->first] = true; + executedTests.insert(it2->first); ++it2; } if (it->second.empty()) { @@ -134,20 +133,14 @@ void DataSelector::Display() { } return; } - - - if (executedTests.size() < 4) { - std::vector executedKeys{}, validKeys{}; + if (executedTests.size() < 4 && !m_testCountValidated) { for (auto test : kValidTests) { - validKeys.push_back(test.first); - } - for (auto test : executedTests) { - executedKeys.push_back(test.first); + if (!executedTests.contains(test)) { + m_missingTests.push_back(test); + m_testCountValidated = true; + } } - - std::ranges::set_difference(executedKeys, validKeys, - std::back_inserter(m_missingTests)); } #if 0 diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h index 3df8a6f7012..9f3f26498da 100644 --- a/sysid/src/main/native/include/sysid/view/DataSelector.h +++ b/sysid/src/main/native/include/sysid/view/DataSelector.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -75,21 +76,13 @@ class DataSelector : public glass::View { int m_selectedAnalysis = 0; std::future m_testdataFuture; std::vector m_testdataStats; - std::map> kValidTests = { - {"quasistatic-forward", true}, - {"quasistatic-reverse", true}, - {"dynamic-forward", true}, - {"dynamic-reverse", true} - }; - std::map> executedTests; //= { - // {"quasistatic-forward", false}, - // {"quasistatic-reverse", false}, - // {"dynamic-forward", false}, - // {"dynamic-reverse", false} - // }; + std::set> kValidTests = { + "quasistatic-forward", "quasistatic-reverse", "dynamic-forward", + "dynamic-reverse"}; + std::set> executedTests; + bool m_testCountValidated = false; static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry); TestData BuildTestData(); - }; } // namespace sysid From ba705e38ff4adb91d5b7822005c4f5dffe1b0b83 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 23:00:12 -0500 Subject: [PATCH 10/14] use default comparison --- sysid/src/main/native/include/sysid/view/DataSelector.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h index 9f3f26498da..65b0d0ab371 100644 --- a/sysid/src/main/native/include/sysid/view/DataSelector.h +++ b/sysid/src/main/native/include/sysid/view/DataSelector.h @@ -76,10 +76,10 @@ class DataSelector : public glass::View { int m_selectedAnalysis = 0; std::future m_testdataFuture; std::vector m_testdataStats; - std::set> kValidTests = { + std::set kValidTests = { "quasistatic-forward", "quasistatic-reverse", "dynamic-forward", "dynamic-reverse"}; - std::set> executedTests; + std::set executedTests; bool m_testCountValidated = false; static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry); From 73edf65d2ac7dc90f515390bac5af99cdfab3911 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 23:04:24 -0500 Subject: [PATCH 11/14] add --- .../main/native/include/sysid/analysis/FilteringUtils.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h index 7f233311874..70cc142b646 100644 --- a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h +++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -75,14 +76,12 @@ class MissingTestsError : public std::exception { public: explicit MissingTestsError(std::vector MissingTests) : missingTests(std::move(MissingTests)) { - errorString = fmt::format( + errorString = fmt::format( "The following tests were not detected: {}. Make sure to perform all " "four tests as described in the SysId documentation.", fmt::join(missingTests, ", ")); - } - const char* what() const noexcept override { - return errorString.c_str(); } + const char* what() const noexcept override { return errorString.c_str(); } private: std::vector missingTests; From 12652aca254eadbcc9795fe3d38ad87051f44527 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 23:10:25 -0500 Subject: [PATCH 12/14] Revert "alignas fail" This reverts commit 5e97940f3460141414e1b1c319041c76cecd9676. --- .../native/cpp/analysis/FeedbackAnalysis.cpp | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 985c597bd8c..7821812bd4e 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -52,14 +52,9 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( auto system = frc::LinearSystem<1, 1, 1>( Eigen::Matrix{0.0}, Eigen::Matrix{1.0}, Eigen::Matrix{1.0}, Eigen::Matrix{0.0}); - - alignas(double) - auto Q = frc::MakeCostMatrix(params.qv); - alignas(double) - auto R = frc::MakeCostMatrix(params.r); // Create an LQR with one state -- position. - frc::LinearQuadraticRegulator<1, 1> controller{system.A(), system.B(), Q, R, - preset.period}; + frc::LinearQuadraticRegulator<1, 1> controller{ + system, {params.qp}, {params.r}, preset.period}; // Compensate for any latency from sensor measurements, filtering, etc. controller.LatencyCompensate(system, preset.period, preset.measurementDelay); @@ -84,13 +79,9 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains( frc::LinearSystem<1, 1, 1> system{ frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka}, frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}}; - alignas(double) - auto Q = frc::MakeCostMatrix(params.qv); - alignas(double) - auto R = frc::MakeCostMatrix(params.r); // Create an LQR controller with 1 state -- velocity. - frc::LinearQuadraticRegulator<1, 1> controller{system.A(), system.B(), Q, R, - preset.period}; + frc::LinearQuadraticRegulator<1, 1> controller{ + system, {params.qv}, {params.r}, preset.period}; // Compensate for any latency from sensor measurements, filtering, etc. controller.LatencyCompensate(system, preset.period, preset.measurementDelay); From 8ee14244e1c07067b2ac93a99f61e3de56df240e Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Thu, 30 Jan 2025 23:28:42 -0500 Subject: [PATCH 13/14] indent validtests --- sysid/src/main/native/cpp/view/DataSelector.cpp | 6 +++--- sysid/src/main/native/include/sysid/view/DataSelector.h | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/sysid/src/main/native/cpp/view/DataSelector.cpp b/sysid/src/main/native/cpp/view/DataSelector.cpp index 51d53c78eec..f3c320c643e 100644 --- a/sysid/src/main/native/cpp/view/DataSelector.cpp +++ b/sysid/src/main/native/cpp/view/DataSelector.cpp @@ -112,7 +112,7 @@ void DataSelector::Display() { continue; } WPI_INFO(m_logger, "Loaded test state {}", it2->first); - executedTests.insert(it2->first); + m_executedTests.insert(it2->first); ++it2; } if (it->second.empty()) { @@ -134,9 +134,9 @@ void DataSelector::Display() { return; } - if (executedTests.size() < 4 && !m_testCountValidated) { + if (m_executedTests.size() < 4 && !m_testCountValidated) { for (auto test : kValidTests) { - if (!executedTests.contains(test)) { + if (!m_executedTests.contains(test)) { m_missingTests.push_back(test); m_testCountValidated = true; } diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h index 65b0d0ab371..e387837595c 100644 --- a/sysid/src/main/native/include/sysid/view/DataSelector.h +++ b/sysid/src/main/native/include/sysid/view/DataSelector.h @@ -76,10 +76,10 @@ class DataSelector : public glass::View { int m_selectedAnalysis = 0; std::future m_testdataFuture; std::vector m_testdataStats; - std::set kValidTests = { - "quasistatic-forward", "quasistatic-reverse", "dynamic-forward", - "dynamic-reverse"}; - std::set executedTests; + std::set kValidTests = {"quasistatic-forward", + "quasistatic-reverse", "dynamic-forward", + "dynamic-reverse"}; + std::set m_executedTests; bool m_testCountValidated = false; static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry); From a8330a03225de4229fc36c44031bfe242471bfbb Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Fri, 31 Jan 2025 10:01:26 -0500 Subject: [PATCH 14/14] format --- sysid/src/main/native/include/sysid/analysis/FilteringUtils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h index 70cc142b646..3948cdd1218 100644 --- a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h +++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h @@ -13,12 +13,12 @@ #include #include +#include #include #include #include #include -#include "fmt/ranges.h" #include "sysid/analysis/AnalysisManager.h" #include "sysid/analysis/Storage.h"