From 49a8eff29860a81614a289d9c975b54ba4707800 Mon Sep 17 00:00:00 2001 From: Oblarg Date: Mon, 28 Feb 2022 13:57:42 -0500 Subject: [PATCH] Add measurement delay estimation (#384) --- .../native/cpp/analysis/AnalysisManager.cpp | 243 ++++++------------ .../native/cpp/analysis/FilteringUtils.cpp | 64 +++-- .../src/main/native/cpp/view/Analyzer.cpp | 42 ++- .../include/sysid/analysis/AnalysisManager.h | 41 ++- .../include/sysid/analysis/FilteringUtils.h | 15 +- .../test/native/cpp/analysis/FilterTest.cpp | 4 +- 6 files changed, 206 insertions(+), 203 deletions(-) diff --git a/sysid-application/src/main/native/cpp/analysis/AnalysisManager.cpp b/sysid-application/src/main/native/cpp/analysis/AnalysisManager.cpp index 37749e19..cd7250e2 100644 --- a/sysid-application/src/main/native/cpp/analysis/AnalysisManager.cpp +++ b/sysid-application/src/main/native/cpp/analysis/AnalysisManager.cpp @@ -94,37 +94,7 @@ static Storage CombineDatasets(const std::vector& slowForward, return Storage{slowForward, slowBackward, fastForward, fastBackward}; } -/** - * Prepares data for general mechanisms (i.e. not drivetrain) and stores them - * in the analysis manager dataset. - * - * @param json A reference to the JSON containing all of the collected - * data. - * @param settings A reference to the settings being used by the analysis - * manager instance. - * @param factor The units per rotation to multiply positions and velocities - * by. - * @param unit The name of the unit being used - * @param originalDataset A reference to the String Map storing the original - * datasets (won't be touched in the filtering process) - * @param rawDataset A reference to the String Map storing the raw datasets - * @param filteredDataset A reference to the String Map storing the filtered - * datasets - * @param startTimes A reference to an array containing the start times for the - * 4 different tests - * @param minStepTime A reference to the minimum duration of the step test as - * one of the trimming procedures will remove this amount - * from the start of the test. - * @param maxStepTime A reference to the maximum duration of the step test - * mainly for use in the GUI - * @param logger A reference to a logger to help with debugging - */ -static void PrepareGeneralData( - const wpi::json& json, AnalysisManager::Settings& settings, double factor, - std::string_view unit, std::array& originalDataset, - std::array& rawDataset, std::array& filteredDataset, - std::array& startTimes, units::second_t& minStepTime, - units::second_t& maxStepTime, wpi::Logger& logger) { +void AnalysisManager::PrepareGeneralData() { using Data = std::array; wpi::StringMap> data; wpi::StringMap> preparedData; @@ -135,27 +105,27 @@ static void PrepareGeneralData( static constexpr size_t kPosCol = 2; static constexpr size_t kVelCol = 3; - WPI_INFO(logger, "{}", "Reading JSON data."); + 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] = json.at(key).get>(); + data[key] = m_json.at(key).get>(); } - WPI_INFO(logger, "{}", "Preprocessing raw data."); + WPI_INFO(m_logger, "{}", "Preprocessing raw data."); // Ensure that voltage and velocity have the same sign. Also multiply // positions and velocities by the factor. for (auto it = data.begin(); it != data.end(); ++it) { for (auto&& pt : it->second) { pt[kVoltageCol] = std::copysign(pt[kVoltageCol], pt[kVelCol]); - pt[kPosCol] *= factor; - pt[kVelCol] *= factor; + pt[kPosCol] *= m_factor; + pt[kVelCol] *= m_factor; } } - WPI_INFO(logger, "{}", "Copying raw data."); + WPI_INFO(m_logger, "{}", "Copying raw data."); CopyRawData(&data); - WPI_INFO(logger, "{}", "Converting raw data to PreparedData struct."); + WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct."); // Convert data to PreparedData structs for (auto& it : data) { auto key = it.first(); @@ -165,73 +135,43 @@ static void PrepareGeneralData( } // Store the original datasets - originalDataset[static_cast( + 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(logger, "{}", "Initial trimming and filtering."); - sysid::InitialTrimAndFilter(&preparedData, &settings, minStepTime, - maxStepTime, unit); + WPI_INFO(m_logger, "{}", "Initial trimming and filtering."); + sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays, + m_velocityDelays, m_minStepTime, m_maxStepTime, + m_unit); - WPI_INFO(logger, "{}", "Acceleration filtering."); + WPI_INFO(m_logger, "{}", "Acceleration filtering."); sysid::AccelFilter(&preparedData); - WPI_INFO(logger, "{}", "Storing datasets."); + WPI_INFO(m_logger, "{}", "Storing datasets."); // Store the raw datasets - rawDataset[static_cast( + 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"]); // Store the filtered datasets - filteredDataset[static_cast( + m_filteredDataset[static_cast( AnalysisManager::Settings::DrivetrainDataset::kCombined)] = CombineDatasets( preparedData["slow-forward"], preparedData["slow-backward"], preparedData["fast-forward"], preparedData["fast-backward"]); - 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}; + 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}; } -/** - * Prepares data for angular drivetrain test data and stores them in - * the analysis manager dataset. - * - * @param json A reference to the JSON containing all of the collected - * data. - * @param settings A reference to the settings being used by the analysis - * manager instance. - * @param factor The units per rotation to multiply positions and velocities - * by. - * @param trackWidth A reference to the std::optional where the track width will - * be stored. - * @param originalDataset A reference to the String Map storing the original - * datasets (won't be touched in the filtering process) - * @param rawDataset A reference to the String Map storing the raw datasets - * @param filteredDataset A reference to the String Map storing the filtered - * datasets - * @param startTimes A reference to an array containing the start times for the - * 4 different tests - * @param minStepTime A reference to the minimum duration of the step test as - * one of the trimming procedures will remove this amount - * from the start of the test. - * @param maxStepTime A reference to the maximum duration of the step test - * mainly for use in the GUI - * @param logger A reference to a logger to help with debugging - */ -static void PrepareAngularDrivetrainData( - const wpi::json& json, AnalysisManager::Settings& settings, double factor, - std::optional& trackWidth, std::array& originalDataset, - std::array& rawDataset, std::array& filteredDataset, - std::array& startTimes, units::second_t& minStepTime, - units::second_t& maxStepTime, wpi::Logger& logger) { +void AnalysisManager::PrepareAngularDrivetrainData() { using Data = std::array; wpi::StringMap> data; wpi::StringMap> preparedData; @@ -245,19 +185,19 @@ static void PrepareAngularDrivetrainData( static constexpr size_t kAngleCol = 7; static constexpr size_t kAngularRateCol = 8; - WPI_INFO(logger, "{}", "Reading JSON data."); + 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] = json.at(key).get>(); + data[key] = m_json.at(key).get>(); } - WPI_INFO(logger, "{}", "Preprocessing raw data."); + WPI_INFO(m_logger, "{}", "Preprocessing raw data."); // Ensure that voltage and velocity have the same sign. Also multiply // positions and velocities by the factor. for (auto it = data.begin(); it != data.end(); ++it) { for (auto&& pt : it->second) { - pt[kLPosCol] *= factor; - pt[kRPosCol] *= factor; + pt[kLPosCol] *= m_factor; + pt[kRPosCol] *= m_factor; // Store the summarized average voltages in the left voltage column pt[kLVoltageCol] = @@ -267,7 +207,7 @@ static void PrepareAngularDrivetrainData( } } - WPI_INFO(logger, "{}", "Calculating trackwidth"); + WPI_INFO(m_logger, "{}", "Calculating trackwidth"); // Aggregating all the deltas from all the tests double leftDelta = 0.0; double rightDelta = 0.0; @@ -282,13 +222,13 @@ static void PrepareAngularDrivetrainData( angleDelta += std::abs(trackWidthData.back()[kAngleCol] - trackWidthData.front()[kAngleCol]); } - trackWidth = sysid::CalculateTrackWidth(leftDelta, rightDelta, - units::radian_t{angleDelta}); + m_trackWidth = sysid::CalculateTrackWidth(leftDelta, rightDelta, + units::radian_t{angleDelta}); - WPI_INFO(logger, "{}", "Copying raw data."); + WPI_INFO(m_logger, "{}", "Copying raw data."); CopyRawData(&data); - WPI_INFO(logger, "{}", "Converting to PreparedData struct."); + WPI_INFO(m_logger, "{}", "Converting to PreparedData struct."); // Convert raw data to prepared data for (const auto& it : data) { auto key = it.first(); @@ -298,7 +238,7 @@ static void PrepareAngularDrivetrainData( // To convert the angular rates into translational velocities (to work with // the model + OLS), v = ωr => v = ω * trackwidth / 2 - double translationalFactor = trackWidth.value() / 2.0; + double translationalFactor = m_trackWidth.value() / 2.0; // Scale Angular Rate Data for (auto& it : preparedData) { @@ -311,69 +251,40 @@ static void PrepareAngularDrivetrainData( } // Create the distinct datasets and store them - originalDataset[static_cast( + 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(logger, "{}", "Applying trimming and filtering."); - sysid::InitialTrimAndFilter(&preparedData, &settings, minStepTime, - maxStepTime); + WPI_INFO(m_logger, "{}", "Applying trimming and filtering."); + sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays, + m_velocityDelays, m_minStepTime, m_maxStepTime); - WPI_INFO(logger, "{}", "Acceleration filtering."); + WPI_INFO(m_logger, "{}", "Acceleration filtering."); sysid::AccelFilter(&preparedData); - WPI_INFO(logger, "{}", "Storing datasets."); + WPI_INFO(m_logger, "{}", "Storing datasets."); // Create the distinct datasets and store them - rawDataset[static_cast( + 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"]); - filteredDataset[static_cast( + m_filteredDataset[static_cast( AnalysisManager::Settings::DrivetrainDataset::kCombined)] = CombineDatasets( preparedData["slow-forward"], preparedData["slow-backward"], preparedData["fast-forward"], preparedData["fast-backward"]); - startTimes = {preparedData["slow-forward"][0].timestamp, - preparedData["slow-backward"][0].timestamp, - preparedData["fast-forward"][0].timestamp, - preparedData["fast-backward"][0].timestamp}; + m_startTimes = {preparedData["slow-forward"][0].timestamp, + preparedData["slow-backward"][0].timestamp, + preparedData["fast-forward"][0].timestamp, + preparedData["fast-backward"][0].timestamp}; } -/** - * Prepares data for linear drivetrain test data and stores them in - * the analysis manager dataset. - * - * @param json A reference to the JSON containing all of the collected - * data. - * @param settings A reference to the settings being used by the analysis - * manager instance. - * @param factor The units per rotation to multiply positions and velocities - * by. - * @param originalDataset A reference to the String Map storing the original - * datasets (won't be touched in the filtering process) - * @param rawDataset A reference to the String Map storing the raw datasets - * @param filteredDataset A reference to the String Map storing the filtered - * datasets - * @param startTimes A reference to an array containing the start times for the - * 4 different tests - * @param minStepTime A reference to the minimum duration of the step test as - * one of the trimming procedures will remove this amount - * from the start of the test. - * @param maxStepTime A reference to the maximum duration of the step test - * mainly for use in the GUI - * @param logger A reference to a logger to help with debugging - */ -static void PrepareLinearDrivetrainData( - const wpi::json& json, AnalysisManager::Settings& settings, double factor, - std::array& originalDataset, std::array& rawDataset, - std::array& filteredDataset, - std::array& startTimes, units::second_t& minStepTime, - units::second_t& maxStepTime, wpi::Logger& logger) { +void AnalysisManager::PrepareLinearDrivetrainData() { using Data = std::array; wpi::StringMap> data; wpi::StringMap> preparedData; @@ -388,30 +299,30 @@ static void PrepareLinearDrivetrainData( static constexpr size_t kRVelCol = 6; // Get the major components from the JSON and store them inside a StringMap. - WPI_INFO(logger, "{}", "Reading JSON data."); + WPI_INFO(m_logger, "{}", "Reading JSON data."); for (auto&& key : AnalysisManager::kJsonDataKeys) { - data[key] = json.at(key).get>(); + 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(logger, "{}", "Preprocessing raw data."); + 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] *= factor; - pt[kRPosCol] *= factor; - pt[kLVelCol] *= factor; - pt[kRVelCol] *= factor; + pt[kLPosCol] *= m_factor; + pt[kRPosCol] *= m_factor; + pt[kLVelCol] *= m_factor; + pt[kRVelCol] *= m_factor; } } - WPI_INFO(logger, "{}", "Copying raw data."); + WPI_INFO(m_logger, "{}", "Copying raw data."); CopyRawData(&data); // Convert data to PreparedData - WPI_INFO(logger, "{}", "Converting to PreparedData struct."); + WPI_INFO(m_logger, "{}", "Converting to PreparedData struct."); for (auto& it : data) { auto key = it.first(); @@ -436,26 +347,26 @@ static void PrepareLinearDrivetrainData( auto originalFastBackward = AnalysisManager::DataConcat( preparedData["left-original-raw-fast-backward"], preparedData["right-original-raw-fast-backward"]); - originalDataset[static_cast( + m_originalDataset[static_cast( AnalysisManager::Settings::DrivetrainDataset::kCombined)] = CombineDatasets(originalSlowForward, originalSlowBackward, originalFastForward, originalFastBackward); - originalDataset[static_cast( + 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"]); - originalDataset[static_cast( + 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(logger, "{}", "Applying trimming and filtering."); - sysid::InitialTrimAndFilter(&preparedData, &settings, minStepTime, - maxStepTime); + 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"]); @@ -466,10 +377,10 @@ static void PrepareLinearDrivetrainData( auto fastBackward = AnalysisManager::DataConcat( preparedData["left-fast-backward"], preparedData["right-fast-backward"]); - WPI_INFO(logger, "{}", "Acceleration filtering."); + WPI_INFO(m_logger, "{}", "Acceleration filtering."); sysid::AccelFilter(&preparedData); - WPI_INFO(logger, "{}", "Storing datasets."); + WPI_INFO(m_logger, "{}", "Storing datasets."); // Create the distinct raw datasets and store them auto rawSlowForward = @@ -485,17 +396,17 @@ static void PrepareLinearDrivetrainData( AnalysisManager::DataConcat(preparedData["left-raw-fast-backward"], preparedData["right-raw-fast-backward"]); - rawDataset[static_cast( + m_rawDataset[static_cast( AnalysisManager::Settings::DrivetrainDataset::kCombined)] = CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward, rawFastBackward); - rawDataset[static_cast( + 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"]); - rawDataset[static_cast( + m_rawDataset[static_cast( AnalysisManager::Settings::DrivetrainDataset::kRight)] = CombineDatasets(preparedData["right-raw-slow-forward"], preparedData["right-raw-slow-backward"], @@ -503,23 +414,23 @@ static void PrepareLinearDrivetrainData( preparedData["right-raw-fast-backward"]); // Create the distinct filtered datasets and store them - filteredDataset[static_cast( + m_filteredDataset[static_cast( AnalysisManager::Settings::DrivetrainDataset::kCombined)] = CombineDatasets(slowForward, slowBackward, fastForward, fastBackward); - filteredDataset[static_cast( + 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"]); - filteredDataset[static_cast( + 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"]); - startTimes = { + m_startTimes = { rawSlowForward.front().timestamp, rawSlowBackward.front().timestamp, rawFastForward.front().timestamp, rawFastBackward.front().timestamp}; } @@ -579,24 +490,16 @@ AnalysisManager::AnalysisManager(std::string_view path, Settings& settings, // Reset settings for Dynamic Test Limits m_settings.stepTestDuration = units::second_t{0.0}; m_settings.motionThreshold = std::numeric_limits::infinity(); - m_minDuration = units::second_t{100000}; } void AnalysisManager::PrepareData() { WPI_INFO(m_logger, "Preparing {} data", m_type.name); if (m_type == analysis::kDrivetrain) { - PrepareLinearDrivetrainData(m_json, m_settings, m_factor, m_originalDataset, - m_rawDataset, m_filteredDataset, m_startTimes, - m_minDuration, m_maxDuration, m_logger); + PrepareLinearDrivetrainData(); } else if (m_type == analysis::kDrivetrainAngular) { - PrepareAngularDrivetrainData(m_json, m_settings, m_factor, m_trackWidth, - m_originalDataset, m_rawDataset, - m_filteredDataset, m_startTimes, m_minDuration, - m_maxDuration, m_logger); + PrepareAngularDrivetrainData(); } else { - PrepareGeneralData(m_json, m_settings, m_factor, m_unit, m_originalDataset, - m_rawDataset, m_filteredDataset, m_startTimes, - m_minDuration, m_maxDuration, m_logger); + PrepareGeneralData(); } WPI_INFO(m_logger, "{}", "Finished Preparing Data"); } diff --git a/sysid-application/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid-application/src/main/native/cpp/analysis/FilteringUtils.cpp index b34fbba6..528fb332 100644 --- a/sysid-application/src/main/native/cpp/analysis/FilteringUtils.cpp +++ b/sysid-application/src/main/native/cpp/analysis/FilteringUtils.cpp @@ -106,18 +106,34 @@ static void PrepareMechData(std::vector* data, } } -units::second_t sysid::TrimStepVoltageData(std::vector* data, - AnalysisManager::Settings* settings, - units::second_t minStepTime, - units::second_t maxStepTime) { - auto firstTimestamp = data->at(0).timestamp; +std::tuple +sysid::TrimStepVoltageData(std::vector* data, + AnalysisManager::Settings* settings, + units::second_t minStepTime, + units::second_t maxStepTime) { + auto voltageBegins = + std::find_if(data->begin(), data->end(), + [](auto& datum) { return std::abs(datum.voltage) > 0; }); + + units::second_t firstTimestamp = voltageBegins->timestamp; + double firstPosition = voltageBegins->position; + + auto motionBegins = std::find_if( + data->begin(), data->end(), [settings, firstPosition](auto& datum) { + return std::abs(datum.position - firstPosition) > + (settings->motionThreshold * datum.dt.value()); + }); + + auto maxAccel = std::max_element( + data->begin(), data->end(), [](const auto& a, const auto& b) { + return std::abs(a.acceleration) < std::abs(b.acceleration); + }); + + units::second_t positionDelay = motionBegins->timestamp - firstTimestamp; + units::second_t velocityDelay = maxAccel->timestamp - firstTimestamp; // Trim data before max acceleration - data->erase(data->begin(), - std::max_element( - data->begin(), data->end(), [](const auto& a, const auto& b) { - return std::abs(a.acceleration) < std::abs(b.acceleration); - })); + data->erase(data->begin(), maxAccel); minStepTime = std::min(data->at(0).timestamp - firstTimestamp, minStepTime); @@ -154,7 +170,7 @@ units::second_t sysid::TrimStepVoltageData(std::vector* data, if (maxIt != data->end()) { data->erase(maxIt, data->end()); } - return minStepTime; + return std::make_tuple(minStepTime, positionDelay, velocityDelay); } double sysid::GetNoiseFloor( @@ -262,31 +278,33 @@ static std::string RemoveStr(std::string_view str, std::string_view removeStr) { * * @return The maximum duration of the Dynamic Tests */ -static units::second_t GetMaxTime( +static units::second_t GetMaxStepTime( wpi::StringMap>& data) { - auto maxDuration = 0_s; + auto maxStepTime = 0_s; for (auto& it : data) { auto key = it.first(); auto& dataset = it.getValue(); if (IsRaw(key) && wpi::contains(key, "fast")) { auto duration = dataset.back().timestamp - dataset.front().timestamp; - if (duration > maxDuration) { - maxDuration = duration; + if (duration > maxStepTime) { + maxStepTime = duration; } } } - return maxDuration; + return maxStepTime; } void sysid::InitialTrimAndFilter( wpi::StringMap>* data, - AnalysisManager::Settings* settings, units::second_t& minStepTime, + AnalysisManager::Settings* settings, + std::vector& positionDelays, + std::vector& velocityDelays, units::second_t& minStepTime, units::second_t& maxStepTime, std::string_view unit) { auto& preparedData = *data; // Find the maximum Step Test Duration of the dynamic tests - maxStepTime = GetMaxTime(preparedData); + maxStepTime = GetMaxStepTime(preparedData); // Calculate Velocity Threshold if it hasn't been set yet if (settings->motionThreshold == std::numeric_limits::infinity()) { @@ -337,9 +355,13 @@ void sysid::InitialTrimAndFilter( auto filteredKey = RemoveStr(key, "raw-"); // Trim Filtered Data - auto tempMinStepTime = TrimStepVoltageData( - &preparedData[filteredKey], settings, minStepTime, maxStepTime); - minStepTime = tempMinStepTime; + auto [tempMinStepTime, positionDelay, velocityDelay] = + TrimStepVoltageData(&preparedData[filteredKey], settings, minStepTime, + maxStepTime); + auto minStepTime = tempMinStepTime; + + positionDelays.emplace_back(positionDelay); + velocityDelays.emplace_back(velocityDelay); // Set the Raw Data to start at the same time as the Filtered Data auto startTime = preparedData[filteredKey].front().timestamp; diff --git a/sysid-application/src/main/native/cpp/view/Analyzer.cpp b/sysid-application/src/main/native/cpp/view/Analyzer.cpp index 2ae3d376..1f172dff 100644 --- a/sysid-application/src/main/native/cpp/view/Analyzer.cpp +++ b/sysid-application/src/main/native/cpp/view/Analyzer.cpp @@ -55,6 +55,10 @@ void Analyzer::UpdateFeedforwardGains() { m_accelRSquared = std::get<1>(ff); m_accelRMSE = std::get<2>(ff); m_trackWidth = trackWidth; + m_settings.preset.measurementDelay = + m_settings.type == FeedbackControllerLoopType::kPosition + ? m_manager->GetPositionDelay() + : m_manager->GetVelocityDelay(); PrepareGraphs(); } catch (const sysid::InvalidDataError& e) { m_state = AnalyzerState::kGeneralDataError; @@ -521,8 +525,8 @@ void Analyzer::DisplayFeedforwardParameters(float beginX, float beginY) { SetPosition(beginX, beginY, kHorizontalOffset, 2); ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); if (ImGui::SliderFloat("Test Duration", &m_stepTestDuration, - m_manager->GetMinDuration(), - m_manager->GetMaxDuration(), "%.2f")) { + m_manager->GetMinStepTime().value(), + m_manager->GetMaxStepTime().value(), "%.2f")) { m_settings.stepTestDuration = units::second_t{m_stepTestDuration}; PrepareData(); } @@ -571,7 +575,27 @@ void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) { "at least 3-5 times shorter than this to optimally control the " "system."); - size_t row = 4; + SetPosition(beginX, beginY, 0, 4); + double positionDelay = m_manager->GetPositionDelay().value(); + DisplayGain("Position Measurement Delay (s)", &positionDelay); + CreateTooltip( + "The average elapsed time between the first application of " + "voltage and the first detected change in mechanism position " + "in the step-voltage tests. This includes CAN delays, and " + "may overestimate the true delay for on-motor-controller " + "feedback loops by up to 20ms."); + + SetPosition(beginX, beginY, 0, 5); + double velocityDelay = m_manager->GetVelocityDelay().value(); + DisplayGain("Velocity Measurement Delay (s)", &velocityDelay); + CreateTooltip( + "The average elapsed time between the first application of " + "voltage and the maximum calculated mechanism acceleration " + "in the step-voltage tests. This includes CAN delays, and " + "may overestimate the true delay for on-motor-controller " + "feedback loops by up to 20ms."); + + size_t row = 6; SetPosition(beginX, beginY, 0, row); @@ -596,6 +620,9 @@ void Analyzer::DisplayFeedbackGains() { if (ImGui::Combo("Gain Preset", &m_selectedPreset, kPresetNames, IM_ARRAYSIZE(kPresetNames))) { m_settings.preset = m_presets[kPresetNames[m_selectedPreset]]; + m_settings.type = FeedbackControllerLoopType::kVelocity; + m_selectedLoopType = + static_cast(FeedbackControllerLoopType::kVelocity); m_settings.convertGainsToEncTicks = m_selectedPreset > 2; UpdateFeedbackGains(); } @@ -735,6 +762,15 @@ void Analyzer::DisplayFeedbackGains() { IM_ARRAYSIZE(kLoopTypes))) { m_settings.type = static_cast(m_selectedLoopType); + if (m_state == AnalyzerState::kWaitingForJSON) { + m_settings.preset.measurementDelay = 0_ms; + } else { + if (m_settings.type == FeedbackControllerLoopType::kPosition) { + m_settings.preset.measurementDelay = m_manager->GetPositionDelay(); + } else { + m_settings.preset.measurementDelay = m_manager->GetVelocityDelay(); + } + } UpdateFeedbackGains(); } diff --git a/sysid-application/src/main/native/include/sysid/analysis/AnalysisManager.h b/sysid-application/src/main/native/include/sysid/analysis/AnalysisManager.h index d324612e..a76fa379 100644 --- a/sysid-application/src/main/native/include/sysid/analysis/AnalysisManager.h +++ b/sysid-application/src/main/native/include/sysid/analysis/AnalysisManager.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -267,7 +268,7 @@ class AnalysisManager { * * @return The minimum step test duration. */ - double GetMinDuration() const { return m_minDuration.value(); } + units::second_t GetMinStepTime() const { return m_minStepTime; } /** * Returns the maximum duration of the Step Voltage Test of the currently @@ -275,7 +276,31 @@ class AnalysisManager { * * @return Maximum step test duration */ - double GetMaxDuration() const { return m_maxDuration.value(); } + units::second_t GetMaxStepTime() const { return m_maxStepTime; } + + /** + * Returns the estimated time delay of the measured position, including + * CAN delays. + * + * @return Position delay + */ + units::second_t GetPositionDelay() const { + return std::accumulate(m_positionDelays.begin(), m_positionDelays.end(), + 0_s) / + m_positionDelays.size(); + } + + /** + * Returns the estimated time delay of the measured velocity, including + * CAN delays. + * + * @return Velocity delay + */ + units::second_t GetVelocityDelay() const { + return std::accumulate(m_velocityDelays.begin(), m_velocityDelays.end(), + 0_s) / + m_positionDelays.size(); + } /** * Returns the different start times of the recorded tests. @@ -308,10 +333,18 @@ class AnalysisManager { std::string m_unit; double m_factor; - units::second_t m_minDuration; - units::second_t m_maxDuration; + 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-application/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid-application/src/main/native/include/sysid/analysis/FilteringUtils.h index 84bd4b89..cd0ef6de 100644 --- a/sysid-application/src/main/native/include/sysid/analysis/FilteringUtils.h +++ b/sysid-application/src/main/native/include/sysid/analysis/FilteringUtils.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -115,10 +116,10 @@ void ApplyMedianFilter(std::vector* data, int window); * @param maxStepTime The maximum step test duration. * @return The updated minimum step test duration. */ -units::second_t TrimStepVoltageData(std::vector* data, - AnalysisManager::Settings* settings, - units::second_t minStepTime, - units::second_t maxStepTime); +std::tuple +TrimStepVoltageData(std::vector* data, + AnalysisManager::Settings* settings, + units::second_t minStepTime, units::second_t maxStepTime); /** * Compute the mean time delta of the given data. @@ -178,6 +179,10 @@ frc::LinearFilter CentralFiniteDifference(units::second_t period) { * @param data A pointer to a data vector recently created by the * ConvertToPrepared method * @param settings A reference to the analysis settings + * @param positionDelays A reference to the vector of computed position signal + * delays. + * @param velocityDelays A reference to the vector of computed velocity signal + * delays. * @param minStepTime A reference to the minimum dynamic test duration as one of * the trimming procedures will remove this amount from the * start of the test. @@ -187,6 +192,8 @@ frc::LinearFilter CentralFiniteDifference(units::second_t period) { */ void InitialTrimAndFilter(wpi::StringMap>* data, AnalysisManager::Settings* settings, + std::vector& positionDelays, + std::vector& velocityDelays, units::second_t& minStepTime, units::second_t& maxStepTime, std::string_view unit = ""); diff --git a/sysid-application/src/test/native/cpp/analysis/FilterTest.cpp b/sysid-application/src/test/native/cpp/analysis/FilterTest.cpp index 56af800a..eba393ee 100644 --- a/sysid-application/src/test/native/cpp/analysis/FilterTest.cpp +++ b/sysid-application/src/test/native/cpp/analysis/FilterTest.cpp @@ -61,7 +61,9 @@ TEST(FilterTest, StepTrim) { auto minTime = maxTime; sysid::AnalysisManager::Settings settings; - minTime = sysid::TrimStepVoltageData(&testData, &settings, minTime, maxTime); + auto [tempMinTime, positionDelay, velocityDelay] = + sysid::TrimStepVoltageData(&testData, &settings, minTime, maxTime); + minTime = tempMinTime; EXPECT_EQ(expectedData[0].acceleration, testData[0].acceleration); EXPECT_EQ(expectedData.back().acceleration, testData.back().acceleration);