From 7c26bc70ab3fae41111b7cfff98c2ee37a6dec00 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 5 Jan 2024 16:24:31 -0800 Subject: [PATCH] [sysid] Load DataLog files directly for analysis (#6103) Co-authored-by: Oblarg --- .../src/main/native/cpp/DataLogThread.cpp | 72 --- .../src/main/native/cpp/DataLogThread.h | 71 --- datalogtool/src/main/native/cpp/Exporter.cpp | 25 +- .../cpp/support/DataLogReaderThread.cpp | 124 +++++ .../glass/support/DataLogReaderThread.h | 101 ++++ sysid/build.gradle | 4 +- sysid/src/main/native/cpp/App.cpp | 67 ++- .../native/cpp/analysis/AnalysisManager.cpp | 497 +++--------------- .../main/native/cpp/analysis/AnalysisType.cpp | 6 - .../native/cpp/analysis/FilteringUtils.cpp | 53 +- .../native/cpp/analysis/JSONConverter.cpp | 164 ------ .../native/cpp/telemetry/TelemetryManager.cpp | 275 ---------- sysid/src/main/native/cpp/view/Analyzer.cpp | 124 +---- .../src/main/native/cpp/view/DataSelector.cpp | 244 +++++++++ .../main/native/cpp/view/JSONConverter.cpp | 64 --- sysid/src/main/native/cpp/view/LogLoader.cpp | 208 ++++++++ sysid/src/main/native/cpp/view/Logger.cpp | 222 -------- .../include/sysid/analysis/AnalysisManager.h | 76 +-- .../include/sysid/analysis/AnalysisType.h | 2 - .../include/sysid/analysis/FilteringUtils.h | 3 + .../include/sysid/analysis/JSONConverter.h | 24 - .../native/include/sysid/analysis/Storage.h | 34 ++ .../sysid/telemetry/TelemetryManager.h | 237 --------- .../main/native/include/sysid/view/Analyzer.h | 26 +- .../native/include/sysid/view/DataSelector.h | 80 +++ .../native/include/sysid/view/JSONConverter.h | 57 -- .../native/include/sysid/view/LogLoader.h | 78 +++ .../main/native/include/sysid/view/Logger.h | 82 --- .../main/native/include/sysid/view/UILayout.h | 9 +- .../native/cpp/analysis/AnalysisTypeTest.cpp | 4 - .../cpp/analysis/FeedforwardAnalysisTest.cpp | 48 -- .../test/native/cpp/analysis/FilterTest.cpp | 20 +- .../main/native/include/wpi/DataLogReader.h | 2 +- 33 files changed, 1088 insertions(+), 2015 deletions(-) delete mode 100644 datalogtool/src/main/native/cpp/DataLogThread.cpp delete mode 100644 datalogtool/src/main/native/cpp/DataLogThread.h create mode 100644 glass/src/lib/native/cpp/support/DataLogReaderThread.cpp create mode 100644 glass/src/lib/native/include/glass/support/DataLogReaderThread.h delete mode 100644 sysid/src/main/native/cpp/analysis/JSONConverter.cpp delete mode 100644 sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp create mode 100644 sysid/src/main/native/cpp/view/DataSelector.cpp delete mode 100644 sysid/src/main/native/cpp/view/JSONConverter.cpp create mode 100644 sysid/src/main/native/cpp/view/LogLoader.cpp delete mode 100644 sysid/src/main/native/cpp/view/Logger.cpp delete mode 100644 sysid/src/main/native/include/sysid/analysis/JSONConverter.h delete mode 100644 sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h create mode 100644 sysid/src/main/native/include/sysid/view/DataSelector.h delete mode 100644 sysid/src/main/native/include/sysid/view/JSONConverter.h create mode 100644 sysid/src/main/native/include/sysid/view/LogLoader.h delete mode 100644 sysid/src/main/native/include/sysid/view/Logger.h diff --git a/datalogtool/src/main/native/cpp/DataLogThread.cpp b/datalogtool/src/main/native/cpp/DataLogThread.cpp deleted file mode 100644 index 90c8b196c62..00000000000 --- a/datalogtool/src/main/native/cpp/DataLogThread.cpp +++ /dev/null @@ -1,72 +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 "DataLogThread.h" - -#include - -DataLogThread::~DataLogThread() { - if (m_thread.joinable()) { - m_active = false; - m_thread.join(); - } -} - -void DataLogThread::ReadMain() { - for (auto record : m_reader) { - if (!m_active) { - break; - } - ++m_numRecords; - if (record.IsStart()) { - wpi::log::StartRecordData data; - if (record.GetStartData(&data)) { - std::scoped_lock lock{m_mutex}; - if (m_entries.find(data.entry) != m_entries.end()) { - fmt::print("...DUPLICATE entry ID, overriding\n"); - } - m_entries[data.entry] = data; - m_entryNames.emplace(data.name, data); - sigEntryAdded(data); - } else { - fmt::print("Start(INVALID)\n"); - } - } else if (record.IsFinish()) { - int entry; - if (record.GetFinishEntry(&entry)) { - std::scoped_lock lock{m_mutex}; - auto it = m_entries.find(entry); - if (it == m_entries.end()) { - fmt::print("...ID not found\n"); - } else { - m_entries.erase(it); - } - } else { - fmt::print("Finish(INVALID)\n"); - } - } else if (record.IsSetMetadata()) { - wpi::log::MetadataRecordData data; - if (record.GetSetMetadataData(&data)) { - std::scoped_lock lock{m_mutex}; - auto it = m_entries.find(data.entry); - if (it == m_entries.end()) { - fmt::print("...ID not found\n"); - } else { - it->second.metadata = data.metadata; - auto nameIt = m_entryNames.find(it->second.name); - if (nameIt != m_entryNames.end()) { - nameIt->second.metadata = data.metadata; - } - } - } else { - fmt::print("SetMetadata(INVALID)\n"); - } - } else if (record.IsControl()) { - fmt::print("Unrecognized control record\n"); - } - } - - sigDone(); - m_done = true; -} diff --git a/datalogtool/src/main/native/cpp/DataLogThread.h b/datalogtool/src/main/native/cpp/DataLogThread.h deleted file mode 100644 index 267aa1fef8b..00000000000 --- a/datalogtool/src/main/native/cpp/DataLogThread.h +++ /dev/null @@ -1,71 +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 -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -class DataLogThread { - public: - explicit DataLogThread(wpi::log::DataLogReader reader) - : m_reader{std::move(reader)}, m_thread{[=, this] { ReadMain(); }} {} - ~DataLogThread(); - - bool IsDone() const { return m_done; } - std::string_view GetBufferIdentifier() const { - return m_reader.GetBufferIdentifier(); - } - unsigned int GetNumRecords() const { return m_numRecords; } - unsigned int GetNumEntries() const { - std::scoped_lock lock{m_mutex}; - return m_entryNames.size(); - } - - // Passes wpi::log::StartRecordData to func - template - void ForEachEntryName(T&& func) { - std::scoped_lock lock{m_mutex}; - for (auto&& kv : m_entryNames) { - func(kv.second); - } - } - - wpi::log::StartRecordData GetEntry(std::string_view name) const { - std::scoped_lock lock{m_mutex}; - auto it = m_entryNames.find(name); - if (it == m_entryNames.end()) { - return {}; - } - return it->second; - } - - const wpi::log::DataLogReader& GetReader() const { return m_reader; } - - // note: these are called on separate thread - wpi::sig::Signal_mt sigEntryAdded; - wpi::sig::Signal_mt<> sigDone; - - private: - void ReadMain(); - - wpi::log::DataLogReader m_reader; - mutable wpi::mutex m_mutex; - std::atomic_bool m_active{true}; - std::atomic_bool m_done{false}; - std::atomic m_numRecords{0}; - std::map> m_entryNames; - wpi::DenseMap m_entries; - std::thread m_thread; -}; diff --git a/datalogtool/src/main/native/cpp/Exporter.cpp b/datalogtool/src/main/native/cpp/Exporter.cpp index 9201c8f7a1d..3c0ade12b89 100644 --- a/datalogtool/src/main/native/cpp/Exporter.cpp +++ b/datalogtool/src/main/native/cpp/Exporter.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -32,11 +33,10 @@ #include #include "App.h" -#include "DataLogThread.h" namespace { struct InputFile { - explicit InputFile(std::unique_ptr datalog); + explicit InputFile(std::unique_ptr datalog); InputFile(std::string_view filename, std::string_view status) : filename{filename}, @@ -47,7 +47,7 @@ struct InputFile { std::string filename; std::string stem; - std::unique_ptr datalog; + std::unique_ptr datalog; std::string status; bool highlight = false; }; @@ -135,7 +135,7 @@ static void RebuildEntryTree() { } } -InputFile::InputFile(std::unique_ptr datalog_) +InputFile::InputFile(std::unique_ptr datalog_) : filename{datalog_->GetBufferIdentifier()}, stem{fs::path{filename}.stem().string()}, datalog{std::move(datalog_)} { @@ -192,7 +192,7 @@ static std::unique_ptr LoadDataLog(std::string_view filename) { } return std::make_unique( - std::make_unique(std::move(reader))); + std::make_unique(std::move(reader))); } void DisplayInputFiles() { @@ -284,9 +284,10 @@ static bool EmitEntry(const std::string& name, Entry& entry) { if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); for (auto inputFile : entry.inputFiles) { - ImGui::Text( - "%s: %s", inputFile->stem.c_str(), - std::string{inputFile->datalog->GetEntry(entry.name).type}.c_str()); + if (auto info = inputFile->datalog->GetEntry(entry.name)) { + ImGui::Text("%s: %s", inputFile->stem.c_str(), + std::string{info->type}.c_str()); + } } ImGui::EndTooltip(); } @@ -300,10 +301,10 @@ static bool EmitEntry(const std::string& name, Entry& entry) { if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); for (auto inputFile : entry.inputFiles) { - ImGui::Text( - "%s: %s", inputFile->stem.c_str(), - std::string{inputFile->datalog->GetEntry(entry.name).metadata} - .c_str()); + if (auto info = inputFile->datalog->GetEntry(entry.name)) { + ImGui::Text("%s: %s", inputFile->stem.c_str(), + std::string{info->metadata}.c_str()); + } } ImGui::EndTooltip(); } diff --git a/glass/src/lib/native/cpp/support/DataLogReaderThread.cpp b/glass/src/lib/native/cpp/support/DataLogReaderThread.cpp new file mode 100644 index 00000000000..9d90bb39662 --- /dev/null +++ b/glass/src/lib/native/cpp/support/DataLogReaderThread.cpp @@ -0,0 +1,124 @@ +// 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 "glass/support/DataLogReaderThread.h" + +#include + +#include +#include + +using namespace glass; + +DataLogReaderThread::~DataLogReaderThread() { + if (m_thread.joinable()) { + m_active = false; + m_thread.join(); + } +} + +void DataLogReaderThread::ReadMain() { + wpi::SmallDenseMap< + int, std::pair>, 8> + schemaEntries; + + for (auto recordIt = m_reader.begin(), recordEnd = m_reader.end(); + recordIt != recordEnd; ++recordIt) { + auto& record = *recordIt; + if (!m_active) { + break; + } + ++m_numRecords; + if (record.IsStart()) { + DataLogReaderEntry data; + if (record.GetStartData(&data)) { + std::scoped_lock lock{m_mutex}; + auto& entryPtr = m_entriesById[data.entry]; + if (entryPtr) { + fmt::print("...DUPLICATE entry ID, overriding\n"); + } + auto [it, isNew] = m_entriesByName.emplace(data.name, data); + if (isNew) { + it->second.ranges.emplace_back(recordIt, recordEnd); + } + entryPtr = &it->second; + if (data.type == "structschema" || + data.type == "proto:FileDescriptorProto") { + schemaEntries.try_emplace(data.entry, entryPtr, + std::span{}); + } + sigEntryAdded(data); + } else { + fmt::print("Start(INVALID)\n"); + } + } else if (record.IsFinish()) { + int entry; + if (record.GetFinishEntry(&entry)) { + std::scoped_lock lock{m_mutex}; + auto it = m_entriesById.find(entry); + if (it == m_entriesById.end()) { + fmt::print("...ID not found\n"); + } else { + it->second->ranges.back().m_end = recordIt; + m_entriesById.erase(it); + } + } else { + fmt::print("Finish(INVALID)\n"); + } + } else if (record.IsSetMetadata()) { + wpi::log::MetadataRecordData data; + if (record.GetSetMetadataData(&data)) { + std::scoped_lock lock{m_mutex}; + auto it = m_entriesById.find(data.entry); + if (it == m_entriesById.end()) { + fmt::print("...ID not found\n"); + } else { + it->second->metadata = data.metadata; + } + } else { + fmt::print("SetMetadata(INVALID)\n"); + } + } else if (record.IsControl()) { + fmt::print("Unrecognized control record\n"); + } else { + auto it = schemaEntries.find(record.GetEntry()); + if (it != schemaEntries.end()) { + it->second.second = record.GetRaw(); + } + } + } + + // build schema databases + for (auto&& schemaPair : schemaEntries) { + auto name = schemaPair.second.first->name; + auto data = schemaPair.second.second; + if (data.empty()) { + continue; + } + if (wpi::starts_with(name, "NT:")) { + name = wpi::drop_front(name, 3); + } + if (wpi::starts_with(name, "/.schema/struct:")) { + auto typeStr = wpi::drop_front(name, 16); + std::string_view schema{reinterpret_cast(data.data()), + data.size()}; + std::string err; + auto desc = m_structDb.Add(typeStr, schema, &err); + if (!desc) { + fmt::print("could not decode struct '{}' schema '{}': {}\n", name, + schema, err); + } + } else if (wpi::starts_with(name, "/.schema/proto:")) { + // protobuf descriptor handling + auto filename = wpi::drop_front(name, 15); + if (!m_protoDb.Add(filename, data)) { + fmt::print("could not decode protobuf '{}' filename '{}'\n", name, + filename); + } + } + } + + sigDone(); + m_done = true; +} diff --git a/glass/src/lib/native/include/glass/support/DataLogReaderThread.h b/glass/src/lib/native/include/glass/support/DataLogReaderThread.h new file mode 100644 index 00000000000..bcbc9623ef8 --- /dev/null +++ b/glass/src/lib/native/include/glass/support/DataLogReaderThread.h @@ -0,0 +1,101 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace glass { + +class DataLogReaderRange { + public: + DataLogReaderRange(wpi::log::DataLogReader::iterator begin, + wpi::log::DataLogReader::iterator end) + : m_begin{begin}, m_end{end} {} + + wpi::log::DataLogReader::iterator begin() const { return m_begin; } + wpi::log::DataLogReader::iterator end() const { return m_end; } + + wpi::log::DataLogReader::iterator m_begin; + wpi::log::DataLogReader::iterator m_end; +}; + +class DataLogReaderEntry : public wpi::log::StartRecordData { + public: + std::vector ranges; // ranges where this entry is valid +}; + +class DataLogReaderThread { + public: + explicit DataLogReaderThread(wpi::log::DataLogReader reader) + : m_reader{std::move(reader)}, m_thread{[this] { ReadMain(); }} {} + ~DataLogReaderThread(); + + bool IsDone() const { return m_done; } + std::string_view GetBufferIdentifier() const { + return m_reader.GetBufferIdentifier(); + } + unsigned int GetNumRecords() const { return m_numRecords; } + unsigned int GetNumEntries() const { + std::scoped_lock lock{m_mutex}; + return m_entriesByName.size(); + } + + // Passes Entry& to func + template + void ForEachEntryName(T&& func) { + std::scoped_lock lock{m_mutex}; + for (auto&& kv : m_entriesByName) { + func(kv.second); + } + } + + const DataLogReaderEntry* GetEntry(std::string_view name) const { + std::scoped_lock lock{m_mutex}; + auto it = m_entriesByName.find(name); + if (it == m_entriesByName.end()) { + return nullptr; + } + return &it->second; + } + + wpi::StructDescriptorDatabase& GetStructDatabase() { return m_structDb; } + wpi::ProtobufMessageDatabase& GetProtobufDatabase() { return m_protoDb; } + + const wpi::log::DataLogReader& GetReader() const { return m_reader; } + + // note: these are called on separate thread + wpi::sig::Signal_mt sigEntryAdded; + wpi::sig::Signal_mt<> sigDone; + + private: + void ReadMain(); + + wpi::log::DataLogReader m_reader; + mutable wpi::mutex m_mutex; + std::atomic_bool m_active{true}; + std::atomic_bool m_done{false}; + std::atomic m_numRecords{0}; + std::map> m_entriesByName; + wpi::DenseMap m_entriesById; + wpi::StructDescriptorDatabase m_structDb; + wpi::ProtobufMessageDatabase m_protoDb; + std::thread m_thread; +}; + +} // namespace glass diff --git a/sysid/build.gradle b/sysid/build.gradle index e6709fc8285..b9689e96b35 100644 --- a/sysid/build.gradle +++ b/sysid/build.gradle @@ -102,8 +102,8 @@ model { 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 @@ model { 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/src/main/native/cpp/App.cpp b/sysid/src/main/native/cpp/App.cpp index 947ea434a65..4a60f79b9af 100644 --- a/sysid/src/main/native/cpp/App.cpp +++ b/sysid/src/main/native/cpp/App.cpp @@ -24,21 +24,20 @@ #include #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 gWindowManager; -glass::Window* gLoggerWindow; +glass::Window* gLogLoaderWindow; +glass::Window* gDataSelectorWindow; glass::Window* gAnalyzerWindow; glass::Window* gProgramLogWindow; static glass::MainMenuBar gMainMenu; -std::unique_ptr gJSONConverter; - glass::LogData gLog; wpi::Logger gLogger; @@ -103,11 +102,23 @@ void Application(std::string_view saveDir) { gWindowManager = std::make_unique(storage); gWindowManager->GlobalInit(); - gLoggerWindow = gWindowManager->AddWindow( - "Logger", std::make_unique(storage, gLogger)); + auto logLoader = std::make_unique(storage, gLogger); + auto dataSelector = std::make_unique(storage, gLogger); + auto analyzer = std::make_unique(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)); - gAnalyzerWindow = gWindowManager->AddWindow( - "Analyzer", std::make_unique(storage, gLogger)); + gDataSelectorWindow = + gWindowManager->AddWindow("Data Selector", std::move(dataSelector)); + + gAnalyzerWindow = gWindowManager->AddWindow("Analyzer", std::move(analyzer)); gProgramLogWindow = gWindowManager->AddWindow( "Program Log", std::make_unique(&gLog)); @@ -115,10 +126,16 @@ void Application(std::string_view saveDir) { // 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 @@ void Application(std::string_view saveDir) { sysid::kProgramLogWindowSize.y); gProgramLogWindow->DisableRenamePopup(); - gJSONConverter = std::make_unique(gLogger); - // Configure save file. gui::ConfigurePlatformSaveFile("sysid.ini"); @@ -157,15 +172,6 @@ void Application(std::string_view saveDir) { 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 @@ void Application(std::string_view saveDir) { 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 f88d0597037..be88b5738c4 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 -#include #include -#include #include #include -#include +#include #include #include #include -#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>& 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 @@ using namespace sysid; * * @return A PreparedData vector */ -template -static std::vector ConvertToPrepared( - const std::vector>& data) { +static std::vector ConvertToPrepared(const MotorData& data) { std::vector prepared; - for (int i = 0; i < static_cast(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(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 @@ static std::vector ConvertToPrepared( * * @param dataset A reference to the dataset being used */ -template -static void CopyRawData( - wpi::StringMap>>* dataset) { +static void CopyRawData(wpi::StringMap* 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 @@ static Storage CombineDatasets(const std::vector& slowForward, } void AnalysisManager::PrepareGeneralData() { - using Data = std::array; - wpi::StringMap> data; wpi::StringMap> preparedData; - // Store the raw data columns. - constexpr size_t kTimeCol = 0; - constexpr size_t kVoltageCol = 1; - constexpr size_t kPosCol = 2; - 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>(); - } - 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( - 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( - 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( - AnalysisManager::Settings::DrivetrainDataset::kCombined)] = - CombineDatasets( - preparedData["slow-forward"], preparedData["slow-backward"], - preparedData["fast-forward"], preparedData["fast-backward"]); - - m_startTimes = {preparedData["raw-slow-forward"][0].timestamp, - preparedData["raw-slow-backward"][0].timestamp, - preparedData["raw-fast-forward"][0].timestamp, - preparedData["raw-fast-backward"][0].timestamp}; -} - -void AnalysisManager::PrepareAngularDrivetrainData() { - using Data = std::array; - wpi::StringMap> data; - wpi::StringMap> preparedData; - - // Store the relevant raw data columns. - constexpr size_t kTimeCol = 0; - constexpr size_t kLVoltageCol = 1; - constexpr size_t kRVoltageCol = 2; - constexpr size_t kLPosCol = 3; - constexpr size_t kRPosCol = 4; - constexpr size_t kLVelCol = 5; - constexpr size_t kRVelCol = 6; - constexpr size_t kAngleCol = 7; - 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>(); - } - - 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( - 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( - 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( - 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; - wpi::StringMap> data; - wpi::StringMap> preparedData; - - // Store the relevant raw data columns. - constexpr size_t kTimeCol = 0; - constexpr size_t kLVoltageCol = 1; - constexpr size_t kRVoltageCol = 2; - constexpr size_t kLPosCol = 3; - constexpr size_t kRPosCol = 4; - constexpr size_t kLVelCol = 5; - 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>(); - } - - // 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( - AnalysisManager::Settings::DrivetrainDataset::kCombined)] = - CombineDatasets(originalSlowForward, originalSlowBackward, - originalFastForward, originalFastBackward); - m_originalDataset[static_cast( - 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( - 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( - AnalysisManager::Settings::DrivetrainDataset::kCombined)] = - CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward, - rawFastBackward); - m_rawDataset[static_cast( - 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( - 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( - AnalysisManager::Settings::DrivetrainDataset::kCombined)] = - CombineDatasets(slowForward, slowBackward, fastForward, fastBackward); - m_filteredDataset[static_cast( - 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( - 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_filteredDataset = CombineDatasets( + preparedData["quasistatic-forward"], preparedData["quasistatic-reverse"], + preparedData["dynamic-forward"], preparedData["dynamic-reverse"]); + + 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 fileBuffer = - wpi::MemoryBuffer::GetFile(path, ec); - if (fileBuffer == nullptr || ec) { - throw FileReadingError(path); - } - - m_json = wpi::json::parse(fileBuffer->GetCharBuffer()); - - 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 fileBuffer = - wpi::MemoryBuffer::GetFile(path, ec); - if (fileBuffer == nullptr || ec) { - throw FileReadingError(newPath); - } - - m_json = wpi::json::parse(fileBuffer->GetCharBuffer()); - - 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()); - - // Get the rotation -> output units factor from the JSON. - m_unit = m_json.at("units").get(); - m_factor = m_json.at("unitsPerRotation").get(); - 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::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,8 +192,9 @@ AnalysisManager::FeedforwardGains AnalysisManager::CalculateFeedforward() { 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 = ff.coeffs[0]; const auto& Kv = ff.coeffs[1]; @@ -542,27 +220,20 @@ sysid::FeedbackGains AnalysisManager::CalculateFeedback( 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(); - m_factor = m_json.at("unitsPerRotation").get(); -} +void AnalysisManager::ResetUnitsFromJSON() {} diff --git a/sysid/src/main/native/cpp/analysis/AnalysisType.cpp b/sysid/src/main/native/cpp/analysis/AnalysisType.cpp index 18b461fe6d8..6ef27c93875 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/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp index 6c66ef8b424..838968fc49b 100644 --- a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp +++ b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp @@ -153,32 +153,24 @@ sysid::TrimStepVoltageData(std::vector* data, minStepTime = std::min(data->at(0).timestamp - firstTimestamp, minStepTime); - // If step duration hasn't been set yet, calculate a default (find the entry - // before the acceleration first hits zero) - if (settings->stepTestDuration <= minStepTime) { - // Get noise floor - const double accelNoiseFloor = GetNoiseFloor( - *data, kNoiseMeanWindow, [](auto&& pt) { return pt.acceleration; }); - // Find latest element with nonzero acceleration - auto endIt = std::find_if( - data->rbegin(), data->rend(), [&](const PreparedData& entry) { - return std::abs(entry.acceleration) > accelNoiseFloor; - }); - - if (endIt != data->rend()) { - // Calculate default duration - settings->stepTestDuration = std::min( - endIt->timestamp - data->front().timestamp + minStepTime + 1_s, - maxStepTime); - } else { - settings->stepTestDuration = maxStepTime; - } + // Find 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->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 @@ sysid::TrimStepVoltageData(std::vector* data, if (maxIt != data->end()) { data->erase(maxIt, data->end()); } + return std::make_tuple(minStepTime, positionDelay, velocityDelay); } @@ -204,6 +197,16 @@ double sysid::GetNoiseFloor( return std::sqrt(sum / (data.size() - step)); } +double sysid::GetMaxSpeed( + const std::vector& data, + std::function 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& data) { std::vector dts; @@ -301,7 +304,7 @@ static units::second_t GetMaxStepTime( 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 @@ void sysid::InitialTrimAndFilter( 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 @@ void sysid::InitialTrimAndFilter( // 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 @@ void sysid::InitialTrimAndFilter( 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 9a5505f4f32..00000000000 --- 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 -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "sysid/Util.h" -#include "sysid/analysis/AnalysisManager.h" -#include "sysid/analysis/AnalysisType.h" - -// Sizes of the arrays for new sysid data. -inline constexpr size_t kDrivetrainSize = 9; -inline constexpr size_t kGeneralSize = 4; - -// Indices for the old data. -inline constexpr size_t kTimestampCol = 0; -inline constexpr size_t kLVoltsCol = 3; -inline constexpr size_t kRVoltsCol = 4; -inline constexpr size_t kLPosCol = 5; -inline constexpr size_t kRPosCol = 6; -inline constexpr size_t kLVelCol = 7; -inline constexpr size_t kRVelCol = 8; - -static wpi::json GetJSON(std::string_view path, wpi::Logger& logger) { - std::error_code ec; - std::unique_ptr 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->GetCharBuffer()); - 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()); - auto factor = ojson.at("unitsPerRotation").get(); - auto unit = ojson.at("units").get(); - - 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> data; - data.reserve(odata.size()); - - // Transfer the data. - for (auto&& pt : odata) { - data.push_back(std::array{ - 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> data; - data.reserve(odata.size()); - - // Transfer the data. - for (auto&& pt : odata) { - data.push_back(std::array{ - 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()); - auto factor = json.at("unitsPerRotation").get(); - auto unit = json.at("units").get(); - 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>>(); - 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>>(); - 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/telemetry/TelemetryManager.cpp b/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp deleted file mode 100644 index ac97cdbca4d..00000000000 --- 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 -#include // for ::tolower -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#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 res; - wpi::split(m_params.raw, res, ','); - - // Convert each string to double. - std::vector values; - values.reserve(res.size()); - for (auto&& str : res) { - values.push_back(wpi::parse_float(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 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 ed73e4bf33c..f121ab51d8b 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 @@ Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger) void Analyzer::UpdateFeedforwardGains() { WPI_INFO(m_logger, "{}", "Gain calc"); try { - const auto& [ff, trackWidth] = m_manager->CalculateFeedforward(); + const auto& [ff] = m_manager->CalculateFeedforward(); m_ff = ff.coeffs; m_accelRSquared = ff.rSquared; m_accelRMSE = ff.rmse; - m_trackWidth = trackWidth; m_settings.preset.measurementDelay = m_settings.type == FeedbackControllerLoopType::kPosition ? m_manager->GetPositionDelay() : m_manager->GetVelocityDelay(); - m_conversionFactor = m_manager->GetFactor(); PrepareGraphs(); } catch (const sysid::InvalidDataError& e) { m_state = AnalyzerState::kGeneralDataError; @@ -81,6 +79,7 @@ void Analyzer::UpdateFeedforwardGains() { } 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 @@ bool Analyzer::IsDataErrorState() { 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( - "Select Data", "", - std::vector{"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(m_settings, m_logger); - m_location = ""; m_ff = std::vector{1, 1, 1}; UpdateFeedbackGains(); } @@ -152,38 +133,15 @@ bool Analyzer::DisplayResetAndUnitOverride() { 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(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 @@ bool Analyzer::DisplayResetAndUnitOverride() { 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 @@ void Analyzer::ConfigParamsOnFileSelect() { WPI_INFO(m_logger, "{}", "Configuring Params"); m_stepTestDuration = m_settings.stepTestDuration.to(); - // 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 @@ void Analyzer::Display() { 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 @@ void Analyzer::Display() { 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::PrepareGraphs() { 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 @@ void Analyzer::DisplayGraphs() { 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(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(m_data, m_settings, m_logger); + PrepareData(); + m_dataset = 0; + ConfigParamsOnFileSelect(); + UpdateFeedbackGains(); } void Analyzer::AbortDataPrep() { @@ -625,8 +545,6 @@ void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) { "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 @@ void Analyzer::DisplayFeedbackGains() { IM_ARRAYSIZE(kLoopTypes))) { m_settings.type = static_cast(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 @@ void Analyzer::DisplayFeedbackGains() { 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 @@ void Analyzer::DisplayFeedbackGains() { } 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 00000000000..c160490889f --- /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 +#include +#include +#include +#include +#include + +#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: (%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(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 +static void AddSample(std::vector>& 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(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 88eaa6a02f0..00000000000 --- 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 - -#include -#include -#include - -#include "sysid/Util.h" - -using namespace sysid; - -void JSONConverter::DisplayConverter( - const char* tooltip, - std::function converter) { - if (ImGui::Button(tooltip)) { - m_opener = std::make_unique( - tooltip, "", std::vector{"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 00000000000..fdaa3af11be --- /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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +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( + "Select Data Log", "", + std::vector{"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(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 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& 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 5e7773dbd0f..00000000000 --- 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 -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#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(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(*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("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/analysis/AnalysisManager.h b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h index 956b3b58be6..d2c38e1aef0 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 #include +#include #include #include "sysid/analysis/AnalysisType.h" @@ -33,6 +34,9 @@ namespace sysid { */ 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 @@ class AnalysisManager { * dataset. */ struct Settings { - enum class DrivetrainDataset { kCombined = 0, kLeft = 1, kRight = 2 }; /** * The feedback controller preset used to calculate gains. */ @@ -87,8 +90,6 @@ class AnalysisManager { * in a smart motor controller). */ bool convertGainsToEncTicks = false; - - DrivetrainDataset dataset = DrivetrainDataset::kCombined; }; /** @@ -99,11 +100,6 @@ class AnalysisManager { * Stores the Feedforward gains. */ OLSResult ffGains; - - /** - * Stores the trackwidth for angular drivetrain tests. - */ - std::optional trackWidth; }; /** @@ -133,7 +129,8 @@ class AnalysisManager { * 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 @@ -170,12 +167,11 @@ class AnalysisManager { * 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 @@ -203,10 +199,8 @@ class AnalysisManager { * 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. @@ -218,21 +212,14 @@ class AnalysisManager { * * @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. @@ -241,9 +228,7 @@ class AnalysisManager { * * @return A reference to the raw internal data. */ - Storage& GetRawData() { - return m_rawDataset[static_cast(m_settings.dataset)]; - } + Storage& GetRawData() { return m_rawDataset; } /** * Returns a reference to the iterator of the currently selected filtered @@ -252,18 +237,14 @@ class AnalysisManager { * * @return A reference to the filtered internal data. */ - Storage& GetFilteredData() { - return m_filteredDataset[static_cast(m_settings.dataset)]; - } + Storage& GetFilteredData() { return m_filteredDataset; } /** * Returns the original dataset. * * @return The original (untouched) dataset */ - Storage& GetOriginalData() { - return m_originalDataset[static_cast(m_settings.dataset)]; - } + Storage& GetOriginalData() { return m_originalDataset; } /** * Returns the minimum duration of the Step Voltage Test of the currently @@ -314,22 +295,14 @@ class AnalysisManager { return m_startTimes; } - bool HasData() const { - return !m_originalDataset[static_cast( - 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 m_originalDataset; - std::array m_rawDataset; - std::array m_filteredDataset; + Storage m_originalDataset; + Storage m_rawDataset; + Storage m_filteredDataset; // Stores the various start times of the different tests. std::array m_startTimes; @@ -338,24 +311,11 @@ class AnalysisManager { // 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::infinity()}; std::vector m_positionDelays; std::vector m_velocityDelays; - // Stores an optional track width if we are doing the drivetrain angular test. - std::optional 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 64ef9f6f8f6..5a30d7ca868 100644 --- a/sysid/src/main/native/include/sysid/analysis/AnalysisType.h +++ b/sysid/src/main/native/include/sysid/analysis/AnalysisType.h @@ -52,8 +52,6 @@ struct AnalysisType { }; namespace analysis { -inline constexpr AnalysisType kDrivetrain{3, 9, "Drivetrain"}; -inline constexpr AnalysisType kDrivetrainAngular{3, 9, "Drivetrain (Angular)"}; inline constexpr AnalysisType kElevator{4, 4, "Elevator"}; inline constexpr AnalysisType kArm{5, 4, "Arm"}; inline constexpr AnalysisType kSimple{3, 4, "Simple"}; diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h index 9ea993da1d3..28538a186e5 100644 --- a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h +++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h @@ -98,6 +98,9 @@ double GetNoiseFloor( const std::vector& data, int window, std::function accessorFunction); +double GetMaxSpeed(const std::vector& data, + std::function 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 7581d25fc16..00000000000 --- 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 -#include - -#include - -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/Storage.h b/sysid/src/main/native/include/sysid/analysis/Storage.h index 52899a0096e..dad38e7cb54 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 #include #include +#include +#include + +#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 + requires std::is_arithmetic_v || units::traits::is_unit_t_v + struct Sample { + Sample(units::second_t time, T measurement) + : time{time}, measurement{measurement} {} + units::second_t time; + T measurement; + }; + std::vector> voltage; + std::vector> position; + std::vector> velocity; + }; + + std::vector runs; +}; + +struct TestData { + std::string distanceUnit; + AnalysisType mechanismType; + wpi::StringMap 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 85ee09e520b..00000000000 --- 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 -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 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> 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 m_tests; - - // Stores the test data. - wpi::json m_data; - - // Display callbacks. - wpi::SmallVector, 1> m_callbacks; - - // NetworkTables instance and entries. - nt::NetworkTableInstance m_inst; - std::shared_ptr 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 2f30f610000..7dcfd41164f 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 @@ namespace sysid { */ 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 @@ class Analyzer : public glass::View { ~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 */ @@ -112,11 +113,6 @@ class Analyzer : public glass::View { */ void DisplayGraphs(); - /** - * Displays the file selection widget. - */ - void DisplayFileSelector(); - /** * Resets the current analysis data. */ @@ -196,7 +192,7 @@ class Analyzer : public glass::View { 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 @@ class Analyzer : public glass::View { double m_Kd; units::millisecond_t m_timescale; - // Track width - std::optional m_trackWidth; - // Units int m_selectedOverrideUnit = 0; - double m_conversionFactor = 0.0; // Data analysis std::unique_ptr 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 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 00000000000..95d7e197d3b --- /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 +#include +#include +#include +#include + +#include +#include +#include + +#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 testdata; + + private: + // wpi::Logger& m_logger; + using Runs = std::vector; + using State = std::map>; // full name + using Tests = std::map>; // e.g. "dynamic" + std::future 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 m_testdataFuture; + std::vector 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 89bfa3290d1..00000000000 --- 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 -#include -#include -#include - -#include -#include -#include - -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 converter); - - wpi::Logger& m_logger; - - std::string m_location; - std::unique_ptr 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 00000000000..04ddd84dab3 --- /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 +#include +#include +#include + +#include +#include + +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 m_opener; + std::unique_ptr 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 children; // children, sorted by name + }; + std::vector m_entryTree; + + void RebuildEntryTree(); + void DisplayEntryTree(const std::vector& 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 d06d6508165..00000000000 --- 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 -#include - -#include -#include -#include -#include -#include - -#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 m_manager = - std::make_unique(m_settings, m_logger); - - std::unique_ptr 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 732a1aaf658..f5d1e440f8f 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 @@ inline constexpr Vector2d kLeftColSize{ 310, kAppWindowSize.y - kLeftColPos.y - kWindowGap}; // Left column contents -inline constexpr Vector2d kLoggerWindowPos = kLeftColPos; -inline constexpr Vector2d kLoggerWindowSize{ - kLeftColSize.x, kAppWindowSize.y - kWindowGap - kLoggerWindowPos.y}; +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 0abb2a1e9cc..51d348ccf37 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 27f7fa686e9..d8cd79eda8d 100644 --- a/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp +++ b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp @@ -241,54 +241,6 @@ TEST(FeedforwardAnalysisTest, Arm) { } } -TEST(FeedforwardAnalysisTest, Drivetrain) { - { - constexpr double Ks = 1.01; - constexpr double Kv = 3.060; - constexpr double Ka = 0.327; - - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - - RunTests(model, sysid::analysis::kDrivetrain, {{Ks, Kv, Ka}}, - {{8e-3, 8e-3, 8e-3}}); - } - - { - constexpr double Ks = 0.547; - constexpr double Kv = 0.0693; - constexpr double Ka = 0.1170; - - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - - RunTests(model, sysid::analysis::kDrivetrain, {{Ks, Kv, Ka}}, - {{8e-3, 8e-3, 8e-3}}); - } -} - -TEST(FeedforwardAnalysisTest, DrivetrainAngular) { - { - constexpr double Ks = 1.01; - constexpr double Kv = 3.060; - constexpr double Ka = 0.327; - - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - - RunTests(model, sysid::analysis::kDrivetrainAngular, {{Ks, Kv, Ka}}, - {{8e-3, 8e-3, 8e-3}}); - } - - { - constexpr double Ks = 0.547; - constexpr double Kv = 0.0693; - constexpr double Ka = 0.1170; - - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - - RunTests(model, sysid::analysis::kDrivetrainAngular, {{Ks, Kv, Ka}}, - {{8e-3, 8e-3, 8e-3}}); - } -} - TEST(FeedforwardAnalysisTest, Elevator) { { constexpr double Ks = 1.01; diff --git a/sysid/src/test/native/cpp/analysis/FilterTest.cpp b/sysid/src/test/native/cpp/analysis/FilterTest.cpp index a7b03492747..8cfebe4f2ac 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, NoiseFloor) { TEST(FilterTest, StepTrim) { std::vector 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 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 @@ TEST(FilterTest, StepTrim) { 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/wpiutil/src/main/native/include/wpi/DataLogReader.h b/wpiutil/src/main/native/include/wpi/DataLogReader.h index b1153e4228d..cb9a8cb8ffb 100644 --- a/wpiutil/src/main/native/include/wpi/DataLogReader.h +++ b/wpiutil/src/main/native/include/wpi/DataLogReader.h @@ -288,7 +288,7 @@ class DataLogIterator { pointer operator->() const { return &this->operator*(); } - private: + protected: const DataLogReader* m_reader; size_t m_pos; mutable bool m_valid = false;