Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[sysid] Load DataLog files directly for analysis (#6103)

Browse Source 
Co-authored-by: Oblarg <emichaelbrnett@gmail.com>
This commit is contained in:
Peter Johnson
2024-01-05 16:24:31 -08:00
committed by GitHub
parent f94e3d81b9
commit 7c26bc70ab
33 changed files with 1086 additions and 2013 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
datalogtool/src/main/native/cpp/DataLogThread.cpp
View File

@@ -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 <fmt/format.h>
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;
}

71
datalogtool/src/main/native/cpp/DataLogThread.h
View File

@@ -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 <atomic>
#include <functional>
#include <map>
#include <string>
#include <string_view>
#include <thread>
#include <utility>
#include <wpi/DataLogReader.h>
#include <wpi/DenseMap.h>
#include <wpi/Signal.h>
#include <wpi/mutex.h>
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 <typename T>
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<const wpi::log::StartRecordData&> 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<unsigned int> m_numRecords{0};
std::map<std::string, wpi::log::StartRecordData, std::less<>> m_entryNames;
wpi::DenseMap<int, wpi::log::StartRecordData> m_entries;
std::thread m_thread;
};

25
datalogtool/src/main/native/cpp/Exporter.cpp
View File

@@ -17,6 +17,7 @@
#include <fmt/chrono.h>
#include <fmt/format.h>
#include <glass/Storage.h>
#include <glass/support/DataLogReaderThread.h>
#include <imgui.h>
#include <imgui_internal.h>
#include <imgui_stdlib.h>
@@ -32,11 +33,10 @@
#include <wpi/raw_ostream.h>
#include "App.h"
#include "DataLogThread.h"
namespace {
struct InputFile {
explicit InputFile(std::unique_ptr<DataLogThread> datalog);
explicit InputFile(std::unique_ptr<glass::DataLogReaderThread> 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<DataLogThread> datalog;
std::unique_ptr<glass::DataLogReaderThread> datalog;
std::string status;
bool highlight = false;
};
@@ -135,7 +135,7 @@ static void RebuildEntryTree() {
}
}
InputFile::InputFile(std::unique_ptr<DataLogThread> datalog_)
InputFile::InputFile(std::unique_ptr<glass::DataLogReaderThread> datalog_)
: filename{datalog_->GetBufferIdentifier()},
stem{fs::path{filename}.stem().string()},
datalog{std::move(datalog_)} {
@@ -192,7 +192,7 @@ static std::unique_ptr<InputFile> LoadDataLog(std::string_view filename) {
}
return std::make_unique<InputFile>(
std::make_unique<DataLogThread>(std::move(reader)));
std::make_unique<glass::DataLogReaderThread>(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();
}

124
glass/src/lib/native/cpp/support/DataLogReaderThread.cpp Normal file
View File

@@ -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 <utility>
#include <fmt/format.h>
#include <wpi/StringExtras.h>
using namespace glass;
DataLogReaderThread::~DataLogReaderThread() {
if (m_thread.joinable()) {
m_active = false;
m_thread.join();
}
}
void DataLogReaderThread::ReadMain() {
wpi::SmallDenseMap<
int, std::pair<DataLogReaderEntry*, std::span<const uint8_t>>, 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<const uint8_t>{});
}
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<const char*>(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;
}

101
glass/src/lib/native/include/glass/support/DataLogReaderThread.h Normal file
View File

@@ -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 <atomic>
#include <functional>
#include <map>
#include <string>
#include <string_view>
#include <thread>
#include <utility>
#include <vector>
#include <wpi/DataLogReader.h>
#include <wpi/DenseMap.h>
#include <wpi/Signal.h>
#include <wpi/mutex.h>
#include <wpi/protobuf/ProtobufMessageDatabase.h>
#include <wpi/struct/DynamicStruct.h>
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<DataLogReaderRange> 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 <typename T>
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<const DataLogReaderEntry&> 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<unsigned int> m_numRecords{0};
std::map<std::string, DataLogReaderEntry, std::less<>> m_entriesByName;
wpi::DenseMap<int, DataLogReaderEntry*> m_entriesById;
wpi::StructDescriptorDatabase m_structDb;
wpi::ProtobufMessageDatabase m_protoDb;
std::thread m_thread;
};
} // namespace glass

4
sysid/build.gradle
View File

@@ -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()) {

67
sysid/src/main/native/cpp/App.cpp
View File

@@ -24,21 +24,20 @@
#include <wpigui_openurl.h>
#include "sysid/view/Analyzer.h"
#include "sysid/view/JSONConverter.h"
#include "sysid/view/Logger.h"
#include "sysid/view/DataSelector.h"
#include "sysid/view/LogLoader.h"
#include "sysid/view/UILayout.h"
namespace gui = wpi::gui;
static std::unique_ptr<glass::WindowManager> gWindowManager;
glass::Window* gLoggerWindow;
glass::Window* gLogLoaderWindow;
glass::Window* gDataSelectorWindow;
glass::Window* gAnalyzerWindow;
glass::Window* gProgramLogWindow;
static glass::MainMenuBar gMainMenu;
std::unique_ptr<sysid::JSONConverter> gJSONConverter;
glass::LogData gLog;
wpi::Logger gLogger;
@@ -103,11 +102,23 @@ void Application(std::string_view saveDir) {
gWindowManager = std::make_unique<glass::WindowManager>(storage);
gWindowManager->GlobalInit();
gLoggerWindow = gWindowManager->AddWindow(
"Logger", std::make_unique<sysid::Logger>(storage, gLogger));
auto logLoader = std::make_unique<sysid::LogLoader>(storage, gLogger);
auto dataSelector = std::make_unique<sysid::DataSelector>(storage, gLogger);
auto analyzer = std::make_unique<sysid::Analyzer>(storage, gLogger);
gAnalyzerWindow = gWindowManager->AddWindow(
"Analyzer", std::make_unique<sysid::Analyzer>(storage, gLogger));
logLoader->unload.connect([ds = dataSelector.get()] { ds->Reset(); });
dataSelector->testdata = [_analyzer = analyzer.get()](auto data) {
_analyzer->m_data = data;
_analyzer->AnalyzeData();
};
gLogLoaderWindow =
gWindowManager->AddWindow("Log Loader", std::move(logLoader));
gDataSelectorWindow =
gWindowManager->AddWindow("Data Selector", std::move(dataSelector));
gAnalyzerWindow = gWindowManager->AddWindow("Analyzer", std::move(analyzer));
gProgramLogWindow = gWindowManager->AddWindow(
"Program Log", std::make_unique<glass::LogView>(&gLog));
@@ -115,10 +126,16 @@ 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<sysid::JSONConverter>(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;

495
sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
View File

@@ -5,24 +5,39 @@
#include "sysid/analysis/AnalysisManager.h"
#include <cmath>
#include <cstddef>
#include <functional>
#include <stdexcept>
#include <fmt/format.h>
#include <units/angle.h>
#include <units/math.h>
#include <wpi/MathExtras.h>
#include <wpi/MemoryBuffer.h>
#include <wpi/StringExtras.h>
#include <wpi/StringMap.h>
#include "sysid/Util.h"
#include "sysid/analysis/FilteringUtils.h"
#include "sysid/analysis/JSONConverter.h"
#include "sysid/analysis/TrackWidthAnalysis.h"
using namespace sysid;
static double Lerp(units::second_t time,
std::vector<MotorData::Run::Sample<double>>& data) {
auto next = std::find_if(data.begin(), data.end(), [&](const auto& entry) {
return entry.time > time;
});
if (next == data.begin()) {
next++;
}
if (next == data.end()) {
next--;
}
const auto prev = next - 1;
return wpi::Lerp(prev->measurement, next->measurement,
(time - prev->time) / (next->time - prev->time));
}
/**
* Converts a raw data vector into a PreparedData vector with only the
* timestamp, voltage, position, and velocity fields filled out.
@@ -38,18 +53,25 @@ using namespace sysid;
*
* @return A PreparedData vector
*/
template <size_t S, size_t Timestamp, size_t Voltage, size_t Position,
size_t Velocity>
static std::vector<PreparedData> ConvertToPrepared(
const std::vector<std::array<double, S>>& data) {
static std::vector<PreparedData> ConvertToPrepared(const MotorData& data) {
std::vector<PreparedData> prepared;
for (int i = 0; i < static_cast<int>(data.size()) - 1; ++i) {
const auto& pt1 = data[i];
const auto& pt2 = data[i + 1];
prepared.emplace_back(PreparedData{
units::second_t{pt1[Timestamp]}, pt1[Voltage], pt1[Position],
pt1[Velocity], units::second_t{pt2[Timestamp] - pt1[Timestamp]}});
// assume we've selected down to a single contiguous run by this point
auto run = data.runs[0];
for (int i = 0; i < static_cast<int>(run.voltage.size()) - 1; ++i) {
const auto& currentVoltage = run.voltage[i];
const auto& nextVoltage = run.voltage[i + 1];
auto currentPosition = Lerp(currentVoltage.time, run.position);
auto currentVelocity = Lerp(currentVoltage.time, run.velocity);
prepared.emplace_back(PreparedData{currentVoltage.time,
currentVoltage.measurement.value(),
currentPosition, currentVelocity,
nextVoltage.time - currentVoltage.time});
}
return prepared;
}
@@ -62,18 +84,16 @@ static std::vector<PreparedData> ConvertToPrepared(
*
* @param dataset A reference to the dataset being used
*/
template <size_t S>
static void CopyRawData(
wpi::StringMap<std::vector<std::array<double, S>>>* dataset) {
static void CopyRawData(wpi::StringMap<MotorData>* dataset) {
auto& data = *dataset;
// Loads the Raw Data
for (auto& it : data) {
auto key = it.first();
auto& dataset = it.getValue();
auto& motorData = it.getValue();
if (!wpi::contains(key, "raw")) {
data[fmt::format("raw-{}", key)] = dataset;
data[fmt::format("original-raw-{}", key)] = dataset;
data[fmt::format("raw-{}", key)] = motorData;
data[fmt::format("original-raw-{}", key)] = motorData;
}
}
}
@@ -94,416 +114,73 @@ static Storage CombineDatasets(const std::vector<PreparedData>& slowForward,
}
void AnalysisManager::PrepareGeneralData() {
using Data = std::array<double, 4>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> preparedData;
// Store the raw data columns.
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<std::vector<Data>>();
}
WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
// Ensure that voltage and velocity have the same sign. Also multiply
// positions and velocities by the factor.
for (auto it = data.begin(); it != data.end(); ++it) {
for (auto&& pt : it->second) {
pt[kVoltageCol] = std::copysign(pt[kVoltageCol], pt[kVelCol]);
pt[kPosCol] *= m_factor;
pt[kVelCol] *= m_factor;
}
}
WPI_INFO(m_logger, "{}", "Copying raw data.");
CopyRawData(&data);
CopyRawData(&m_data.motorData);
WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct.");
// Convert data to PreparedData structs
for (auto& it : data) {
for (auto& it : m_data.motorData) {
auto key = it.first();
preparedData[key] =
ConvertToPrepared<4, kTimeCol, kVoltageCol, kPosCol, kVelCol>(
data[key]);
preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
}
// Store the original datasets
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(preparedData["original-raw-slow-forward"],
preparedData["original-raw-slow-backward"],
preparedData["original-raw-fast-forward"],
preparedData["original-raw-fast-backward"]);
m_originalDataset =
CombineDatasets(preparedData["original-raw-quasistatic-forward"],
preparedData["original-raw-quasistatic-reverse"],
preparedData["original-raw-dynamic-forward"],
preparedData["original-raw-dynamic-reverse"]);
WPI_INFO(m_logger, "{}", "Initial trimming and filtering.");
sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
m_velocityDelays, m_minStepTime, m_maxStepTime,
m_unit);
m_data.distanceUnit);
WPI_INFO(m_logger, "{}", m_minStepTime);
WPI_INFO(m_logger, "{}", m_maxStepTime);
WPI_INFO(m_logger, "{}", "Acceleration filtering.");
sysid::AccelFilter(&preparedData);
WPI_INFO(m_logger, "{}", "Storing datasets.");
// Store the raw datasets
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(
preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
m_rawDataset = CombineDatasets(preparedData["raw-quasistatic-forward"],
preparedData["raw-quasistatic-reverse"],
preparedData["raw-dynamic-forward"],
preparedData["raw-dynamic-reverse"]);
// Store the filtered datasets
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(
preparedData["slow-forward"], preparedData["slow-backward"],
preparedData["fast-forward"], preparedData["fast-backward"]);
m_filteredDataset = CombineDatasets(
preparedData["quasistatic-forward"], preparedData["quasistatic-reverse"],
preparedData["dynamic-forward"], preparedData["dynamic-reverse"]);
m_startTimes = {preparedData["raw-slow-forward"][0].timestamp,
preparedData["raw-slow-backward"][0].timestamp,
preparedData["raw-fast-forward"][0].timestamp,
preparedData["raw-fast-backward"][0].timestamp};
}
void AnalysisManager::PrepareAngularDrivetrainData() {
using Data = std::array<double, 9>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> preparedData;
// Store the relevant raw data columns.
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<std::vector<Data>>();
}
WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
// Ensure that voltage and velocity have the same sign. Also multiply
// positions and velocities by the factor.
for (auto it = data.begin(); it != data.end(); ++it) {
for (auto&& pt : it->second) {
pt[kLPosCol] *= m_factor;
pt[kRPosCol] *= m_factor;
pt[kLVelCol] *= m_factor;
pt[kRVelCol] *= m_factor;
// Stores the average voltages in the left voltage column.
// This aggregates the left and right voltages into a single voltage
// column for the ConvertToPrepared() method. std::copysign() ensures the
// polarity of the voltage matches the direction the robot turns.
pt[kLVoltageCol] = std::copysign(
(std::abs(pt[kLVoltageCol]) + std::abs(pt[kRVoltageCol])) / 2,
pt[kAngularRateCol]);
// ω = (v_r - v_l) / trackwidth
// v = ωr => v = ω * trackwidth / 2
// (v_r - v_l) / trackwidth * (trackwidth / 2) = (v_r - v_l) / 2
// However, since we know this is an angular test, the left and right
// wheel velocities will have opposite signs, allowing us to add their
// absolute values and get the same result (in terms of magnitude).
// std::copysign() is used to make sure the direction of the wheel
// velocities matches the direction the robot turns.
pt[kAngularRateCol] =
std::copysign((std::abs(pt[kRVelCol]) + std::abs(pt[kLVelCol])) / 2,
pt[kAngularRateCol]);
}
}
WPI_INFO(m_logger, "{}", "Calculating trackwidth");
// Aggregating all the deltas from all the tests
double leftDelta = 0.0;
double rightDelta = 0.0;
double angleDelta = 0.0;
for (const auto& it : data) {
auto key = it.first();
auto& trackWidthData = data[key];
leftDelta += std::abs(trackWidthData.back()[kLPosCol] -
trackWidthData.front()[kLPosCol]);
rightDelta += std::abs(trackWidthData.back()[kRPosCol] -
trackWidthData.front()[kRPosCol]);
angleDelta += std::abs(trackWidthData.back()[kAngleCol] -
trackWidthData.front()[kAngleCol]);
}
m_trackWidth = sysid::CalculateTrackWidth(leftDelta, rightDelta,
units::radian_t{angleDelta});
WPI_INFO(m_logger, "{}", "Copying raw data.");
CopyRawData(&data);
WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
// Convert raw data to prepared data
for (const auto& it : data) {
auto key = it.first();
preparedData[key] = ConvertToPrepared<9, kTimeCol, kLVoltageCol, kAngleCol,
kAngularRateCol>(data[key]);
}
// Create the distinct datasets and store them
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(preparedData["original-raw-slow-forward"],
preparedData["original-raw-slow-backward"],
preparedData["original-raw-fast-forward"],
preparedData["original-raw-fast-backward"]);
WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
m_velocityDelays, m_minStepTime, m_maxStepTime);
WPI_INFO(m_logger, "{}", "Acceleration filtering.");
sysid::AccelFilter(&preparedData);
WPI_INFO(m_logger, "{}", "Storing datasets.");
// Create the distinct datasets and store them
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(
preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(
preparedData["slow-forward"], preparedData["slow-backward"],
preparedData["fast-forward"], preparedData["fast-backward"]);
m_startTimes = {preparedData["slow-forward"][0].timestamp,
preparedData["slow-backward"][0].timestamp,
preparedData["fast-forward"][0].timestamp,
preparedData["fast-backward"][0].timestamp};
}
void AnalysisManager::PrepareLinearDrivetrainData() {
using Data = std::array<double, 9>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> preparedData;
// Store the relevant raw data columns.
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<std::vector<Data>>();
}
// Ensure that voltage and velocity have the same sign. Also multiply
// positions and velocities by the factor.
WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
for (auto it = data.begin(); it != data.end(); ++it) {
for (auto&& pt : it->second) {
pt[kLVoltageCol] = std::copysign(pt[kLVoltageCol], pt[kLVelCol]);
pt[kRVoltageCol] = std::copysign(pt[kRVoltageCol], pt[kRVelCol]);
pt[kLPosCol] *= m_factor;
pt[kRPosCol] *= m_factor;
pt[kLVelCol] *= m_factor;
pt[kRVelCol] *= m_factor;
}
}
WPI_INFO(m_logger, "{}", "Copying raw data.");
CopyRawData(&data);
// Convert data to PreparedData
WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
for (auto& it : data) {
auto key = it.first();
preparedData[fmt::format("left-{}", key)] =
ConvertToPrepared<9, kTimeCol, kLVoltageCol, kLPosCol, kLVelCol>(
data[key]);
preparedData[fmt::format("right-{}", key)] =
ConvertToPrepared<9, kTimeCol, kRVoltageCol, kRPosCol, kRVelCol>(
data[key]);
}
// Create the distinct raw datasets and store them
auto originalSlowForward = AnalysisManager::DataConcat(
preparedData["left-original-raw-slow-forward"],
preparedData["right-original-raw-slow-forward"]);
auto originalSlowBackward = AnalysisManager::DataConcat(
preparedData["left-original-raw-slow-backward"],
preparedData["right-original-raw-slow-backward"]);
auto originalFastForward = AnalysisManager::DataConcat(
preparedData["left-original-raw-fast-forward"],
preparedData["right-original-raw-fast-forward"]);
auto originalFastBackward = AnalysisManager::DataConcat(
preparedData["left-original-raw-fast-backward"],
preparedData["right-original-raw-fast-backward"]);
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(originalSlowForward, originalSlowBackward,
originalFastForward, originalFastBackward);
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
CombineDatasets(preparedData["left-original-raw-slow-forward"],
preparedData["left-original-raw-slow-backward"],
preparedData["left-original-raw-fast-forward"],
preparedData["left-original-raw-fast-backward"]);
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kRight)] =
CombineDatasets(preparedData["right-original-raw-slow-forward"],
preparedData["right-original-raw-slow-backward"],
preparedData["right-original-raw-fast-forward"],
preparedData["right-original-raw-fast-backward"]);
WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
m_velocityDelays, m_minStepTime, m_maxStepTime);
auto slowForward = AnalysisManager::DataConcat(
preparedData["left-slow-forward"], preparedData["right-slow-forward"]);
auto slowBackward = AnalysisManager::DataConcat(
preparedData["left-slow-backward"], preparedData["right-slow-backward"]);
auto fastForward = AnalysisManager::DataConcat(
preparedData["left-fast-forward"], preparedData["right-fast-forward"]);
auto fastBackward = AnalysisManager::DataConcat(
preparedData["left-fast-backward"], preparedData["right-fast-backward"]);
WPI_INFO(m_logger, "{}", "Acceleration filtering.");
sysid::AccelFilter(&preparedData);
WPI_INFO(m_logger, "{}", "Storing datasets.");
// Create the distinct raw datasets and store them
auto rawSlowForward =
AnalysisManager::DataConcat(preparedData["left-raw-slow-forward"],
preparedData["right-raw-slow-forward"]);
auto rawSlowBackward =
AnalysisManager::DataConcat(preparedData["left-raw-slow-backward"],
preparedData["right-raw-slow-backward"]);
auto rawFastForward =
AnalysisManager::DataConcat(preparedData["left-raw-fast-forward"],
preparedData["right-raw-fast-forward"]);
auto rawFastBackward =
AnalysisManager::DataConcat(preparedData["left-raw-fast-backward"],
preparedData["right-raw-fast-backward"]);
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward,
rawFastBackward);
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
CombineDatasets(preparedData["left-raw-slow-forward"],
preparedData["left-raw-slow-backward"],
preparedData["left-raw-fast-forward"],
preparedData["left-raw-fast-backward"]);
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kRight)] =
CombineDatasets(preparedData["right-raw-slow-forward"],
preparedData["right-raw-slow-backward"],
preparedData["right-raw-fast-forward"],
preparedData["right-raw-fast-backward"]);
// Create the distinct filtered datasets and store them
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(slowForward, slowBackward, fastForward, fastBackward);
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
CombineDatasets(preparedData["left-slow-forward"],
preparedData["left-slow-backward"],
preparedData["left-fast-forward"],
preparedData["left-fast-backward"]);
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kRight)] =
CombineDatasets(preparedData["right-slow-forward"],
preparedData["right-slow-backward"],
preparedData["right-fast-forward"],
preparedData["right-fast-backward"]);
m_startTimes = {
rawSlowForward.front().timestamp, rawSlowBackward.front().timestamp,
rawFastForward.front().timestamp, rawFastBackward.front().timestamp};
m_startTimes = {preparedData["raw-quasistatic-forward"][0].timestamp,
preparedData["raw-quasistatic-reverse"][0].timestamp,
preparedData["raw-dynamic-forward"][0].timestamp,
preparedData["raw-dynamic-reverse"][0].timestamp};
}
AnalysisManager::AnalysisManager(Settings& settings, wpi::Logger& logger)
: m_logger{logger},
m_settings{settings},
m_type{analysis::kSimple},
m_unit{"Meters"},
m_factor{1} {}
: m_logger{logger}, m_settings{settings} {}
AnalysisManager::AnalysisManager(std::string_view path, Settings& settings,
AnalysisManager::AnalysisManager(TestData data, Settings& settings,
wpi::Logger& logger)
: m_logger{logger}, m_settings{settings} {
{
// Read JSON from the specified path
std::error_code ec;
std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
wpi::MemoryBuffer::GetFile(path, ec);
if (fileBuffer == nullptr || ec) {
throw FileReadingError(path);
}
m_json = wpi::json::parse(fileBuffer->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<wpi::MemoryBuffer> 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<std::string>());
// Get the rotation -> output units factor from the JSON.
m_unit = m_json.at("units").get<std::string>();
m_factor = m_json.at("unitsPerRotation").get<double>();
WPI_DEBUG(m_logger, "Parsing units per rotation as {} {} per rotation",
m_factor, m_unit);
: m_data{std::move(data)}, m_logger{logger}, m_settings{settings} {
// Reset settings for Dynamic Test Limits
m_settings.stepTestDuration = units::second_t{0.0};
m_settings.motionThreshold = std::numeric_limits<double>::infinity();
}
void AnalysisManager::PrepareData() {
WPI_INFO(m_logger, "Preparing {} data", m_type.name);
if (m_type == analysis::kDrivetrain) {
PrepareLinearDrivetrainData();
} else if (m_type == analysis::kDrivetrainAngular) {
PrepareAngularDrivetrainData();
} else {
PrepareGeneralData();
}
// WPI_INFO(m_logger, "Preparing {} data", m_data.mechanismType.name);
PrepareGeneralData();
WPI_INFO(m_logger, "{}", "Finished Preparing Data");
}
@@ -515,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<std::string>();
m_factor = m_json.at("unitsPerRotation").get<double>();
}
void AnalysisManager::ResetUnitsFromJSON() {}

6
sysid/src/main/native/cpp/analysis/AnalysisType.cpp
View File

@@ -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;
}

51
sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
View File

@@ -153,32 +153,24 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* 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;
});
// Find maximum speed reached
const auto maxSpeed =
GetMaxSpeed(*data, [](auto&& pt) { return pt.velocity; });
// Find place where 90% of maximum speed exceeded
auto endIt =
std::find_if(data->begin(), data->end(), [&](const PreparedData& entry) {
return std::abs(entry.velocity) > maxSpeed * 0.9;
});
if (endIt != data->rend()) {
// Calculate default duration
settings->stepTestDuration = std::min(
endIt->timestamp - data->front().timestamp + minStepTime + 1_s,
maxStepTime);
} else {
settings->stepTestDuration = maxStepTime;
}
if (endIt != data->end()) {
settings->stepTestDuration = std::min(
endIt->timestamp - data->front().timestamp + minStepTime, maxStepTime);
}
// Find first entry greater than the step test duration
auto maxIt =
std::find_if(data->begin(), data->end(), [&](PreparedData entry) {
return entry.timestamp - data->front().timestamp + minStepTime >
return entry.timestamp - data->front().timestamp >
settings->stepTestDuration;
});
@@ -186,6 +178,7 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* 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<PreparedData>& data,
std::function<double(const PreparedData&)> accessorFunction) {
double max = 0.0;
for (size_t i = 0; i < data.size(); i++) {
max = std::max(max, std::abs(accessorFunction(data[i])));
}
return max;
}
units::second_t sysid::GetMeanTimeDelta(const std::vector<PreparedData>& data) {
std::vector<units::second_t> dts;
@@ -301,7 +304,7 @@ 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-");

164
sysid/src/main/native/cpp/analysis/JSONConverter.cpp
View File

@@ -1,164 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "sysid/analysis/JSONConverter.h"
#include <stdexcept>
#include <string>
#include <fmt/core.h>
#include <fmt/format.h>
#include <wpi/Logger.h>
#include <wpi/MemoryBuffer.h>
#include <wpi/fmt/raw_ostream.h>
#include <wpi/json.h>
#include <wpi/raw_ostream.h>
#include "sysid/Util.h"
#include "sysid/analysis/AnalysisManager.h"
#include "sysid/analysis/AnalysisType.h"
// Sizes of the arrays for new sysid data.
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<wpi::MemoryBuffer> fileBuffer =
wpi::MemoryBuffer::GetFile(path, ec);
if (fileBuffer == nullptr || ec) {
throw std::runtime_error(fmt::format("Unable to read: {}", path));
}
wpi::json json = wpi::json::parse(fileBuffer->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<std::string>());
auto factor = ojson.at("unitsPerRotation").get<double>();
auto unit = ojson.at("units").get<std::string>();
wpi::json json;
for (auto&& key : AnalysisManager::kJsonDataKeys) {
if (type == analysis::kDrivetrain) {
// Get the old data; create a vector for the new data; reserve the
// appropriate size for the new data.
auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
std::vector<std::array<double, kDrivetrainSize>> data;
data.reserve(odata.size());
// Transfer the data.
for (auto&& pt : odata) {
data.push_back(std::array<double, kDrivetrainSize>{
pt[kTimestampCol], pt[kLVoltsCol], pt[kRVoltsCol], pt[kLPosCol],
pt[kRPosCol], pt[kLVelCol], pt[kRVelCol], 0.0, 0.0});
}
json[key] = data;
} else {
// Get the old data; create a vector for the new data; reserve the
// appropriate size for the new data.
auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
std::vector<std::array<double, kGeneralSize>> data;
data.reserve(odata.size());
// Transfer the data.
for (auto&& pt : odata) {
data.push_back(std::array<double, kGeneralSize>{
pt[kTimestampCol], pt[kLVoltsCol], pt[kLPosCol], pt[kLVelCol]});
}
json[key] = data;
}
}
json["units"] = unit;
json["unitsPerRotation"] = factor;
json["test"] = type.name;
json["sysid"] = true;
// Write the new file with "_new" appended to it.
path.remove_suffix(std::string_view{".json"}.size());
std::string loc = fmt::format("{}_new.json", path);
sysid::SaveFile(json.dump(2), std::filesystem::path{loc});
WPI_INFO(logger, "Wrote new JSON to: {}", loc);
return loc;
}
std::string sysid::ToCSV(std::string_view path, wpi::Logger& logger) {
wpi::json json = GetJSON(path, logger);
auto type = sysid::analysis::FromName(json.at("test").get<std::string>());
auto factor = json.at("unitsPerRotation").get<double>();
auto unit = json.at("units").get<std::string>();
std::string_view abbreviation = GetAbbreviation(unit);
std::error_code ec;
// Naming: {sysid-json-name}(Test, Units).csv
path.remove_suffix(std::string_view{".json"}.size());
std::string loc = fmt::format("{} ({}, {}).csv", path, type.name, unit);
wpi::raw_fd_ostream outputFile{loc, ec};
if (ec) {
throw std::runtime_error("Unable to write to: " + loc);
}
fmt::print(outputFile, "Timestamp (s),Test,");
if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
fmt::print(
outputFile,
"Left Volts (V),Right Volts (V),Left Position ({0}),Right "
"Position ({0}),Left Velocity ({0}/s),Right Velocity ({0}/s),Gyro "
"Position (deg),Gyro Rate (deg/s)\n",
abbreviation);
} else {
fmt::print(outputFile, "Volts (V),Position({0}),Velocity ({0}/s)\n",
abbreviation);
}
outputFile << "\n";
for (auto&& key : AnalysisManager::kJsonDataKeys) {
if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
auto tempData =
json.at(key).get<std::vector<std::array<double, kDrivetrainSize>>>();
for (auto&& pt : tempData) {
fmt::print(outputFile, "{},{},{},{},{},{},{},{},{},{}\n",
pt[0], // Timestamp
key, // Test
pt[1], pt[2], // Left and Right Voltages
pt[3] * factor, pt[4] * factor, // Left and Right Positions
pt[5] * factor, pt[6] * factor, // Left and Right Velocity
pt[7], pt[8] // Gyro Position and Velocity
);
}
} else {
auto tempData =
json.at(key).get<std::vector<std::array<double, kGeneralSize>>>();
for (auto&& pt : tempData) {
fmt::print(outputFile, "{},{},{},{},{}\n",
pt[0], // Timestamp,
key, // Test
pt[1], // Voltage
pt[2] * factor, // Position
pt[3] * factor // Velocity
);
}
}
}
outputFile.flush();
WPI_INFO(logger, "Wrote CSV to: {}", loc);
return loc;
}

275
sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp
View File

@@ -1,275 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "sysid/telemetry/TelemetryManager.h"
#include <algorithm>
#include <cctype> // for ::tolower
#include <numbers>
#include <stdexcept>
#include <string>
#include <utility>
#include <fmt/chrono.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/Logger.h>
#include <wpi/SmallVector.h>
#include <wpi/StringExtras.h>
#include <wpi/raw_ostream.h>
#include <wpi/timestamp.h>
#include "sysid/Util.h"
#include "sysid/analysis/AnalysisType.h"
using namespace sysid;
TelemetryManager::TelemetryManager(const Settings& settings,
wpi::Logger& logger,
nt::NetworkTableInstance instance)
: m_settings(settings), m_logger(logger), m_inst(instance) {}
void TelemetryManager::BeginTest(std::string_view name) {
// Create a new test params instance for this test.
m_params =
TestParameters{name.starts_with("fast"), name.ends_with("forward"),
m_settings.mechanism == analysis::kDrivetrainAngular,
State::WaitingForEnable};
// Add this test to the list of running tests and set the running flag.
m_tests.push_back(std::string{name});
m_isRunningTest = true;
// Set the Voltage Command Entry
m_voltageCommand.Set((m_params.fast ? m_settings.stepVoltage
: m_settings.quasistaticRampRate) *
(m_params.forward ? 1 : -1));
// Set the test type
m_testType.Set(m_params.fast ? "Dynamic" : "Quasistatic");
// Set the rotate entry
m_rotate.Set(m_params.rotate);
// Set the current mechanism in NT.
m_mechanism.Set(m_settings.mechanism.name);
// Set Overflow to False
m_overflowPub.Set(false);
// Set Mechanism Error to False
m_mechErrorPub.Set(false);
m_inst.Flush();
// Display the warning message.
for (auto&& func : m_callbacks) {
func(
"Please enable the robot in autonomous mode, and then "
"disable it "
"before it runs out of space. \n Note: The robot will "
"continue "
"to move until you disable it - It is your "
"responsibility to "
"ensure it does not hit anything!");
}
WPI_INFO(m_logger, "Started {} test.", m_tests.back());
}
void TelemetryManager::EndTest() {
// If there is no test running, this is a no-op
if (!m_isRunningTest) {
return;
}
// Disable the running flag and store the data in the JSON.
m_isRunningTest = false;
m_data[m_tests.back()] = m_params.data;
// Call the cancellation callbacks.
for (auto&& func : m_callbacks) {
std::string msg;
if (m_params.mechError) {
msg +=
"\nERROR: The robot indicated that you are using the wrong project "
"for characterizing your mechanism. \nThis most likely means you "
"are trying to characterize a mechanism like a Drivetrain with a "
"deployed config for a General Mechanism (e.g. Arm, Flywheel, and "
"Elevator) or vice versa. Please double check your settings and "
"try again.";
} else if (!m_params.data.empty()) {
std::string units = m_settings.units;
std::transform(m_settings.units.begin(), m_settings.units.end(),
units.begin(), ::tolower);
if (std::string_view{m_settings.mechanism.name}.starts_with(
"Drivetrain")) {
double p = (m_params.data.back()[3] - m_params.data.front()[3]) *
m_settings.unitsPerRotation;
double s = (m_params.data.back()[4] - m_params.data.front()[4]) *
m_settings.unitsPerRotation;
double g = m_params.data.back()[7] - m_params.data.front()[7];
msg = fmt::format(
"The left and right encoders traveled {} {} and {} {} "
"respectively.\nThe gyro angle delta was {} degrees.",
p, units, s, units, g * 180.0 / std::numbers::pi);
} else {
double p = (m_params.data.back()[2] - m_params.data.front()[2]) *
m_settings.unitsPerRotation;
msg = fmt::format("The encoder reported traveling {} {}.", p, units);
}
if (m_params.overflow) {
msg +=
"\nNOTE: the robot stopped recording data early because the entry "
"storage was exceeded.";
}
} else {
msg = "No data was detected.";
}
func(msg);
}
// Remove previously run test from list of tests if no data was detected.
if (m_params.data.empty()) {
m_tests.pop_back();
}
// Send a zero command over NT.
m_voltageCommand.Set(0.0);
m_inst.Flush();
}
void TelemetryManager::Update() {
// If there is no test running, these is nothing to update.
if (!m_isRunningTest) {
return;
}
// Update the NT entries that we're reading.
int currAckNumber = m_ackNumberSub.Get();
std::string telemetryValue;
// Get the FMS Control Word.
for (auto tsValue : m_fmsControlData.ReadQueue()) {
uint32_t ctrl = tsValue.value;
m_params.enabled = ctrl & 0x01;
}
// Get the string in the data field.
for (auto tsValue : m_telemetry.ReadQueue()) {
telemetryValue = tsValue.value;
}
// Get the overflow flag
for (auto tsValue : m_overflowSub.ReadQueue()) {
m_params.overflow = tsValue.value;
}
// Get the mechanism error flag
for (auto tsValue : m_mechErrorSub.ReadQueue()) {
m_params.mechError = tsValue.value;
}
// Go through our state machine.
if (m_params.state == State::WaitingForEnable) {
if (m_params.enabled) {
m_params.enableStart = wpi::Now() * 1E-6;
m_params.state = State::RunningTest;
m_ackNumber = currAckNumber;
WPI_INFO(m_logger, "{}", "Transitioned to running test state.");
}
}
if (m_params.state == State::RunningTest) {
// If for some reason we've disconnected, end the test.
if (!m_inst.IsConnected()) {
WPI_WARNING(m_logger, "{}",
"NT connection was dropped when executing the test. The test "
"has been canceled.");
EndTest();
}
// If the robot has disabled, then we can move on to the next step.
if (!m_params.enabled) {
m_params.disableStart = wpi::Now() * 1E-6;
m_params.state = State::WaitingForData;
WPI_INFO(m_logger, "{}", "Transitioned to waiting for data.");
}
}
if (m_params.state == State::WaitingForData) {
double now = wpi::Now() * 1E-6;
m_voltageCommand.Set(0.0);
m_inst.Flush();
// Process valid data
if (!telemetryValue.empty() && m_ackNumber < currAckNumber) {
m_params.raw = std::move(telemetryValue);
m_ackNumber = currAckNumber;
}
// We have the data that we need, so we can parse it and end the test.
if (!m_params.raw.empty() &&
wpi::starts_with(m_params.raw, m_tests.back())) {
// Remove test type from start of string
m_params.raw.erase(0, m_params.raw.find(';') + 1);
// Clean up the string -- remove spaces if there are any.
m_params.raw.erase(
std::remove_if(m_params.raw.begin(), m_params.raw.end(), ::isspace),
m_params.raw.end());
// Split the string into individual components.
wpi::SmallVector<std::string_view, 16> res;
wpi::split(m_params.raw, res, ',');
// Convert each string to double.
std::vector<double> values;
values.reserve(res.size());
for (auto&& str : res) {
values.push_back(wpi::parse_float<double>(str).value());
}
// Add the values to our result vector.
for (size_t i = 0; i < values.size() - m_settings.mechanism.rawDataSize;
i += m_settings.mechanism.rawDataSize) {
std::vector<double> d(m_settings.mechanism.rawDataSize);
std::copy_n(std::make_move_iterator(values.begin() + i),
m_settings.mechanism.rawDataSize, d.begin());
m_params.data.push_back(std::move(d));
}
WPI_INFO(m_logger,
"Received data with size: {} for the {} test in {} seconds.",
m_params.data.size(), m_tests.back(),
m_params.data.back()[0] - m_params.data.front()[0]);
m_ackNumberPub.Set(++m_ackNumber);
EndTest();
}
// If we timed out, end the test and let the user know.
if (now - m_params.disableStart > 5.0) {
WPI_WARNING(m_logger, "{}",
"TelemetryManager did not receieve data 5 seconds after "
"completing the test...");
EndTest();
}
}
}
std::string TelemetryManager::SaveJSON(std::string_view location) {
m_data["test"] = m_settings.mechanism.name;
m_data["units"] = m_settings.units;
m_data["unitsPerRotation"] = m_settings.unitsPerRotation;
m_data["sysid"] = true;
std::string loc = fmt::format("{}/sysid_data{:%Y%m%d-%H%M%S}.json", location,
std::chrono::system_clock::now());
sysid::SaveFile(m_data.dump(2), std::filesystem::path{loc});
WPI_INFO(m_logger, "Wrote JSON to: {}", loc);
return loc;
}

124
sysid/src/main/native/cpp/view/Analyzer.cpp
View File

@@ -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<pfd::open_file>(
"Select Data", "",
std::vector<std::string>{"JSON File", SYSID_PFD_JSON_EXT});
}
ImGui::SameLine();
ImGui::SetNextItemWidth(width - ImGui::CalcTextSize("Select").x -
ImGui::GetFontSize() * 5);
ImGui::InputText("##location", &m_location, ImGuiInputTextFlags_ReadOnly);
}
void Analyzer::ResetData() {
m_plot.ResetData();
m_manager = std::make_unique<AnalysisManager>(m_settings, m_logger);
m_location = "";
m_ff = std::vector<double>{1, 1, 1};
UpdateFeedbackGains();
}
@@ -152,38 +133,15 @@ 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<AnalysisManager::Settings::DrivetrainDataset>(m_dataset);
PrepareData();
}
ImGui::SameLine();
} else {
m_settings.dataset =
AnalysisManager::Settings::DrivetrainDataset::kCombined;
}
ImGui::Spacing();
ImGui::Text(
"Units: %s\n"
"Units Per Rotation: %.4f\n"
"Type: %s",
std::string(unit).c_str(), m_conversionFactor, type.name);
if (type == analysis::kDrivetrainAngular) {
ImGui::SameLine();
sysid::CreateTooltip(
"Here, the units and units per rotation represent what the wheel "
"positions and velocities were captured in. The track width value "
"will reflect the unit selected here. However, the Kv and Ka will "
"always be in Vs/rad and Vs^2 / rad respectively.");
}
std::string(unit).c_str(), type.name);
if (ImGui::Button("Override Units")) {
ImGui::OpenPopup("Override Units");
@@ -197,24 +155,11 @@ 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<float>();
// Estimate qp as 1/8 * units-per-rot
m_settings.lqr.qp = 0.125 * m_manager->GetFactor();
// Estimate qp as 1/10 native distance unit
m_settings.lqr.qp = 0.1;
// Estimate qv as 1/4 * max velocity = 1/4 * (12V - kS) / kV
m_settings.lqr.qv = 0.25 * (12.0 - m_ff[0]) / m_ff[1];
}
void Analyzer::Display() {
DisplayFileSelector();
DisplayGraphs();
switch (m_state) {
case AnalyzerState::kWaitingForJSON: {
case AnalyzerState::kWaitingForData: {
ImGui::Text(
"SysId is currently in theoretical analysis mode.\n"
"To analyze recorded test data, select a "
"data JSON.");
"data file (.wpilog).");
sysid::CreateTooltip(
"Theoretical feedback gains can be calculated from a "
"physical model of the mechanism being controlled. "
@@ -295,7 +239,7 @@ 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<AnalysisManager>(m_location, m_settings, m_logger);
PrepareData();
m_dataset = 0;
m_settings.dataset =
AnalysisManager::Settings::DrivetrainDataset::kCombined;
ConfigParamsOnFileSelect();
UpdateFeedbackGains();
}
void Analyzer::AnalyzeData() {
m_manager = std::make_unique<AnalysisManager>(m_data, m_settings, m_logger);
PrepareData();
m_dataset = 0;
ConfigParamsOnFileSelect();
UpdateFeedbackGains();
}
void Analyzer::AbortDataPrep() {
@@ -625,8 +545,6 @@ 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<FeedbackControllerLoopType>(m_selectedLoopType);
if (m_state == AnalyzerState::kWaitingForJSON) {
if (m_state == AnalyzerState::kWaitingForData) {
m_settings.preset.measurementDelay = 0_ms;
} else {
if (m_settings.type == FeedbackControllerLoopType::kPosition) {
@@ -817,7 +735,7 @@ 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()));
}

244
sysid/src/main/native/cpp/view/DataSelector.cpp Normal file
View File

@@ -0,0 +1,244 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "sysid/view/DataSelector.h"
#include <fmt/format.h>
#include <glass/support/DataLogReaderThread.h>
#include <imgui.h>
#include <wpi/DataLogReader.h>
#include <wpi/Logger.h>
#include <wpi/StringExtras.h>
#include "sysid/Util.h"
#include "sysid/analysis/AnalysisType.h"
#include "sysid/analysis/Storage.h"
using namespace sysid;
static constexpr const char* kAnalysisTypes[] = {"Elevator", "Arm", "Simple"};
static bool EmitEntryTarget(const char* name, bool isString,
const glass::DataLogReaderEntry** entry) {
if (*entry) {
auto text =
fmt::format("{}: {} ({})", name, (*entry)->name, (*entry)->type);
ImGui::TextUnformatted(text.c_str());
} else {
ImGui::Text("%s: <none (DROP HERE)> (%s)", name,
isString ? "string" : "number");
}
bool rv = false;
if (ImGui::BeginDragDropTarget()) {
if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload(
isString ? "DataLogEntryString" : "DataLogEntry")) {
assert(payload->DataSize == sizeof(const glass::DataLogReaderEntry*));
*entry = *static_cast<const glass::DataLogReaderEntry**>(payload->Data);
rv = true;
}
ImGui::EndDragDropTarget();
}
return rv;
}
void DataSelector::Display() {
using namespace std::chrono_literals;
// building test data is modal (due to async access)
if (m_testdataFuture.valid()) {
if (m_testdataFuture.wait_for(0s) == std::future_status::ready) {
TestData data = m_testdataFuture.get();
for (auto&& motordata : data.motorData) {
m_testdataStats.emplace_back(
fmt::format("Test State: {}", motordata.first()));
int i = 0;
for (auto&& run : motordata.second.runs) {
m_testdataStats.emplace_back(fmt::format(
" Run {} samples: {} Volt {} Pos {} Vel", ++i,
run.voltage.size(), run.position.size(), run.velocity.size()));
}
}
if (testdata) {
testdata(std::move(data));
}
}
ImGui::Text("Loading data...");
return;
}
if (!m_testdataStats.empty()) {
for (auto&& line : m_testdataStats) {
ImGui::TextUnformatted(line.c_str());
}
if (ImGui::Button("Ok")) {
m_testdataStats.clear();
}
return;
}
if (EmitEntryTarget("Test State", true, &m_testStateEntry)) {
m_testsFuture =
std::async(std::launch::async, [testStateEntry = m_testStateEntry] {
return LoadTests(*testStateEntry);
});
}
if (!m_testStateEntry) {
return;
}
if (m_testsFuture.valid() &&
m_testsFuture.wait_for(0s) == std::future_status::ready) {
m_tests = m_testsFuture.get();
}
if (m_tests.empty()) {
if (m_testsFuture.valid()) {
ImGui::TextUnformatted("Reading tests...");
} else {
ImGui::TextUnformatted("No tests found");
}
return;
}
#if 0
// Test filtering
if (ImGui::BeginCombo("Test", m_selectedTest.c_str())) {
for (auto&& test : m_tests) {
if (ImGui::Selectable(test.first.c_str(), test.first == m_selectedTest)) {
m_selectedTest = test.first;
}
}
ImGui::EndCombo();
}
#endif
ImGui::Combo("Analysis Type", &m_selectedAnalysis, kAnalysisTypes,
IM_ARRAYSIZE(kAnalysisTypes));
// DND targets
EmitEntryTarget("Velocity", false, &m_velocityEntry);
EmitEntryTarget("Position", false, &m_positionEntry);
EmitEntryTarget("Voltage", false, &m_voltageEntry);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
ImGui::Combo("Units", &m_selectedUnit, kUnits, IM_ARRAYSIZE(kUnits));
ImGui::InputDouble("Velocity scaling", &m_velocityScale);
ImGui::InputDouble("Position scaling", &m_positionScale);
if (/*!m_selectedTest.empty() &&*/ m_velocityEntry && m_positionEntry &&
m_voltageEntry) {
if (ImGui::Button("Load")) {
m_testdataFuture =
std::async(std::launch::async, [this] { return BuildTestData(); });
}
}
}
void DataSelector::Reset() {
m_testsFuture = {};
m_tests.clear();
m_selectedTest.clear();
m_testStateEntry = nullptr;
m_velocityEntry = nullptr;
m_positionEntry = nullptr;
m_voltageEntry = nullptr;
m_testdataFuture = {};
}
DataSelector::Tests DataSelector::LoadTests(
const glass::DataLogReaderEntry& testStateEntry) {
Tests tests;
for (auto&& range : testStateEntry.ranges) {
std::string_view prevState;
Runs* curRuns = nullptr;
wpi::log::DataLogReader::iterator lastStart = range.begin();
for (auto it = range.begin(), end = range.end(); it != end; ++it) {
std::string_view testState;
if (it->GetEntry() != testStateEntry.entry ||
!it->GetString(&testState)) {
continue;
}
// track runs as iterator ranges of the same test
if (testState != prevState) {
if (curRuns) {
curRuns->emplace_back(lastStart, it);
}
lastStart = it;
}
prevState = testState;
if (testState == "none") {
curRuns = nullptr;
continue;
}
auto [testName, direction] = wpi::rsplit(testState, '-');
auto testIt = tests.find(testName);
if (testIt == tests.end()) {
testIt = tests.emplace(std::string{testName}, State{}).first;
}
auto stateIt = testIt->second.find(testState);
if (stateIt == testIt->second.end()) {
stateIt = testIt->second.emplace(std::string{testState}, Runs{}).first;
}
curRuns = &stateIt->second;
}
if (curRuns) {
curRuns->emplace_back(lastStart, range.end());
}
}
return tests;
}
template <typename T>
static void AddSample(std::vector<MotorData::Run::Sample<T>>& samples,
const wpi::log::DataLogRecord& record, bool isDouble,
double scale) {
if (isDouble) {
double val;
if (record.GetDouble(&val)) {
samples.emplace_back(units::second_t{record.GetTimestamp() * 1.0e-6},
T{val * scale});
}
} else {
float val;
if (record.GetFloat(&val)) {
samples.emplace_back(units::second_t{record.GetTimestamp() * 1.0e-6},
T{static_cast<double>(val * scale)});
}
}
}
TestData DataSelector::BuildTestData() {
TestData data;
data.distanceUnit = kUnits[m_selectedUnit];
data.mechanismType = analysis::FromName(kAnalysisTypes[m_selectedAnalysis]);
bool voltageDouble = m_voltageEntry->type == "double";
bool positionDouble = m_positionEntry->type == "double";
bool velocityDouble = m_velocityEntry->type == "double";
for (auto&& test : m_tests) {
for (auto&& state : test.second) {
auto& motorData = data.motorData[state.first];
for (auto&& range : state.second) {
auto& run = motorData.runs.emplace_back();
for (auto&& record : range) {
if (record.GetEntry() == m_voltageEntry->entry) {
AddSample(run.voltage, record, voltageDouble, 1.0);
} else if (record.GetEntry() == m_positionEntry->entry) {
AddSample(run.position, record, positionDouble, m_positionScale);
} else if (record.GetEntry() == m_velocityEntry->entry) {
AddSample(run.velocity, record, velocityDouble, m_velocityScale);
}
}
}
}
}
return data;
}

64
sysid/src/main/native/cpp/view/JSONConverter.cpp
View File

@@ -1,64 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "sysid/analysis/JSONConverter.h"
#include "sysid/view/JSONConverter.h"
#include <exception>
#include <imgui.h>
#include <portable-file-dialogs.h>
#include <wpi/timestamp.h>
#include "sysid/Util.h"
using namespace sysid;
void JSONConverter::DisplayConverter(
const char* tooltip,
std::function<std::string(std::string_view, wpi::Logger&)> converter) {
if (ImGui::Button(tooltip)) {
m_opener = std::make_unique<pfd::open_file>(
tooltip, "", std::vector<std::string>{"JSON File", SYSID_PFD_JSON_EXT});
}
if (m_opener && m_opener->ready()) {
if (!m_opener->result().empty()) {
m_location = m_opener->result()[0];
try {
converter(m_location, m_logger);
m_timestamp = wpi::Now() * 1E-6;
} catch (const std::exception& e) {
ImGui::OpenPopup("Exception Caught!");
m_exception = e.what();
}
}
m_opener.reset();
}
if (wpi::Now() * 1E-6 - m_timestamp < 5) {
ImGui::SameLine();
ImGui::Text("Saved!");
}
// Handle exceptions.
ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
if (ImGui::BeginPopupModal("Exception Caught!")) {
ImGui::PushTextWrapPos(0.0f);
ImGui::Text(
"An error occurred when parsing the JSON. This most likely means that "
"the JSON data is incorrectly formatted.");
ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s",
m_exception.c_str());
ImGui::PopTextWrapPos();
if (ImGui::Button("Close")) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
}
void JSONConverter::DisplayCSVConvert() {
DisplayConverter("Select SysId JSON", sysid::ToCSV);
}

208
sysid/src/main/native/cpp/view/LogLoader.cpp Normal file
View File

@@ -0,0 +1,208 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "sysid/view/LogLoader.h"
#include <algorithm>
#include <memory>
#include <span>
#include <string_view>
#include <glass/support/DataLogReaderThread.h>
#include <imgui.h>
#include <imgui_stdlib.h>
#include <portable-file-dialogs.h>
#include <wpi/SpanExtras.h>
#include <wpi/StringExtras.h>
#include <wpi/fs.h>
using namespace sysid;
LogLoader::LogLoader(glass::Storage& storage, wpi::Logger& logger) {}
LogLoader::~LogLoader() = default;
void LogLoader::Display() {
if (ImGui::Button("Open data log file...")) {
m_opener = std::make_unique<pfd::open_file>(
"Select Data Log", "",
std::vector<std::string>{"DataLog Files", "*.wpilog"});
}
// Handle opening the file
if (m_opener && m_opener->ready(0)) {
if (!m_opener->result().empty()) {
m_filename = m_opener->result()[0];
std::error_code ec;
auto buf = wpi::MemoryBuffer::GetFile(m_filename, ec);
if (ec) {
ImGui::OpenPopup("Error");
m_error = fmt::format("Could not open file: {}", ec.message());
return;
}
wpi::log::DataLogReader reader{std::move(buf)};
if (!reader.IsValid()) {
ImGui::OpenPopup("Error");
m_error = "Not a valid datalog file";
return;
}
unload();
m_reader =
std::make_unique<glass::DataLogReaderThread>(std::move(reader));
m_entryTree.clear();
}
m_opener.reset();
}
// Handle errors
ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
if (ImGui::BeginPopupModal("Error")) {
ImGui::PushTextWrapPos(0.0f);
ImGui::TextUnformatted(m_error.c_str());
ImGui::PopTextWrapPos();
if (ImGui::Button("Close")) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
if (!m_reader) {
return;
}
// Summary info
ImGui::TextUnformatted(fs::path{m_filename}.stem().string().c_str());
ImGui::Text("%u records, %u entries%s", m_reader->GetNumRecords(),
m_reader->GetNumEntries(),
m_reader->IsDone() ? "" : " (working)");
if (!m_reader->IsDone()) {
return;
}
bool refilter = ImGui::InputText("Filter", &m_filter);
// Display tree of entries
if (m_entryTree.empty() || refilter) {
RebuildEntryTree();
}
ImGui::BeginTable(
"Entries", 2,
ImGuiTableFlags_Borders | ImGuiTableFlags_SizingStretchProp);
ImGui::TableSetupColumn("Name");
ImGui::TableSetupColumn("Type");
// ImGui::TableSetupColumn("Metadata");
ImGui::TableHeadersRow();
DisplayEntryTree(m_entryTree);
ImGui::EndTable();
}
void LogLoader::RebuildEntryTree() {
m_entryTree.clear();
wpi::SmallVector<std::string_view, 16> parts;
m_reader->ForEachEntryName([&](const glass::DataLogReaderEntry& entry) {
// only show double/float/string entries (TODO: support struct/protobuf)
if (entry.type != "double" && entry.type != "float" &&
entry.type != "string") {
return;
}
// filter on name
if (!m_filter.empty() && !wpi::contains_lower(entry.name, m_filter)) {
return;
}
parts.clear();
// split on first : if one is present
auto [prefix, mainpart] = wpi::split(entry.name, ':');
if (mainpart.empty() || wpi::contains(prefix, '/')) {
mainpart = entry.name;
} else {
parts.emplace_back(prefix);
}
wpi::split(mainpart, parts, '/', -1, false);
// ignore a raw "/" key
if (parts.empty()) {
return;
}
// get to leaf
auto nodes = &m_entryTree;
for (auto part : wpi::drop_back(std::span{parts.begin(), parts.end()})) {
auto it =
std::find_if(nodes->begin(), nodes->end(),
[&](const auto& node) { return node.name == part; });
if (it == nodes->end()) {
nodes->emplace_back(part);
// path is from the beginning of the string to the end of the current
// part; this works because part is a reference to the internals of
// entry.name
nodes->back().path.assign(
entry.name.data(), part.data() + part.size() - entry.name.data());
it = nodes->end() - 1;
}
nodes = &it->children;
}
auto it = std::find_if(nodes->begin(), nodes->end(), [&](const auto& node) {
return node.name == parts.back();
});
if (it == nodes->end()) {
nodes->emplace_back(parts.back());
// no need to set path, as it's identical to entry.name
it = nodes->end() - 1;
}
it->entry = &entry;
});
}
static void EmitEntry(const std::string& name,
const glass::DataLogReaderEntry& entry) {
ImGui::TableNextColumn();
ImGui::Selectable(name.c_str());
if (ImGui::BeginDragDropSource()) {
auto entryPtr = &entry;
ImGui::SetDragDropPayload(
entry.type == "string" ? "DataLogEntryString" : "DataLogEntry",
&entryPtr,
sizeof(entryPtr)); // NOLINT
ImGui::TextUnformatted(entry.name.data(),
entry.name.data() + entry.name.size());
ImGui::EndDragDropSource();
}
ImGui::TableNextColumn();
ImGui::TextUnformatted(entry.type.data(),
entry.type.data() + entry.type.size());
#if 0
ImGui::TableNextColumn();
ImGui::TextUnformatted(entry.metadata.data(),
entry.metadata.data() + entry.metadata.size());
#endif
}
void LogLoader::DisplayEntryTree(const std::vector<EntryTreeNode>& tree) {
for (auto&& node : tree) {
if (node.entry) {
EmitEntry(node.name, *node.entry);
}
if (!node.children.empty()) {
ImGui::TableNextColumn();
bool open = ImGui::TreeNodeEx(node.name.c_str(),
ImGuiTreeNodeFlags_SpanFullWidth);
ImGui::TableNextColumn();
#if 0
ImGui::TableNextColumn();
#endif
if (open) {
DisplayEntryTree(node.children);
ImGui::TreePop();
}
}
}
}

222
sysid/src/main/native/cpp/view/Logger.cpp
View File

@@ -1,222 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "sysid/view/Logger.h"
#include <exception>
#include <numbers>
#include <glass/Context.h>
#include <glass/Storage.h>
#include <imgui.h>
#include <imgui_internal.h>
#include <imgui_stdlib.h>
#include <networktables/NetworkTable.h>
#include <units/angle.h>
#include <wpigui.h>
#include "sysid/Util.h"
#include "sysid/analysis/AnalysisType.h"
#include "sysid/view/UILayout.h"
using namespace sysid;
Logger::Logger(glass::Storage& storage, wpi::Logger& logger)
: m_logger{logger}, m_ntSettings{"sysid", storage} {
wpi::gui::AddEarlyExecute([&] { m_ntSettings.Update(); });
m_ntSettings.EnableServerOption(false);
}
void Logger::Display() {
// Get the current width of the window. This will be used to scale
// our UI elements.
float width = ImGui::GetContentRegionAvail().x;
// Add team number input and apply button for NT connection.
m_ntSettings.Display();
// Reset and clear the internal manager state.
ImGui::SameLine();
if (ImGui::Button("Reset Telemetry")) {
m_settings = TelemetryManager::Settings{};
m_manager = std::make_unique<TelemetryManager>(m_settings, m_logger);
m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]);
}
// Add NT connection indicator.
static ImVec4 kColorDisconnected{1.0f, 0.4f, 0.4f, 1.0f};
static ImVec4 kColorConnected{0.2f, 1.0f, 0.2f, 1.0f};
ImGui::SameLine();
bool ntConnected = nt::NetworkTableInstance::GetDefault().IsConnected();
ImGui::TextColored(ntConnected ? kColorConnected : kColorDisconnected,
ntConnected ? "NT Connected" : "NT Disconnected");
// Create a Section for project configuration
ImGui::Separator();
ImGui::Spacing();
ImGui::Text("Project Parameters");
// Add a dropdown for mechanism type.
ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
if (ImGui::Combo("Mechanism", &m_selectedType, kTypes,
IM_ARRAYSIZE(kTypes))) {
m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]);
}
// Add Dropdown for Units
ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
if (ImGui::Combo("Unit Type", &m_selectedUnit, kUnits,
IM_ARRAYSIZE(kUnits))) {
m_settings.units = kUnits[m_selectedUnit];
}
sysid::CreateTooltip(
"This is the type of units that your gains will be in. For example, if "
"you want your flywheel gains in terms of radians, then use the radians "
"unit. On the other hand, if your drivetrain will use gains in meters, "
"choose meters.");
// Rotational units have fixed Units per rotations
m_isRotationalUnits =
(m_settings.units == "Rotations" || m_settings.units == "Degrees" ||
m_settings.units == "Radians");
if (m_settings.units == "Degrees") {
m_settings.unitsPerRotation = 360.0;
} else if (m_settings.units == "Radians") {
m_settings.unitsPerRotation = 2 * std::numbers::pi;
} else if (m_settings.units == "Rotations") {
m_settings.unitsPerRotation = 1.0;
}
// Units Per Rotations entry
ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
ImGui::InputDouble("Units Per Rotation", &m_settings.unitsPerRotation, 0.0f,
0.0f, "%.4f",
m_isRotationalUnits ? ImGuiInputTextFlags_ReadOnly
: ImGuiInputTextFlags_None);
sysid::CreateTooltip(
"The logger assumes that the code will be sending recorded motor shaft "
"rotations over NetworkTables. This value will then be multiplied by the "
"units per rotation to get the measurement in the units you "
"specified.\n\nFor non-rotational units (e.g. meters), this value is "
"usually the wheel diameter times pi (should not include gearing).");
// Create a section for voltage parameters.
ImGui::Separator();
ImGui::Spacing();
ImGui::Text("Voltage Parameters");
auto CreateVoltageParameters = [this](const char* text, double* data,
float min, float max) {
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
ImGui::PushItemFlag(ImGuiItemFlags_Disabled,
m_manager && m_manager->IsActive());
float value = static_cast<float>(*data);
if (ImGui::SliderFloat(text, &value, min, max, "%.2f")) {
*data = value;
}
ImGui::PopItemFlag();
};
CreateVoltageParameters("Quasistatic Ramp Rate (V/s)",
&m_settings.quasistaticRampRate, 0.10f, 0.60f);
sysid::CreateTooltip(
"This is the rate at which the voltage will increase during the "
"quasistatic test.");
CreateVoltageParameters("Dynamic Step Voltage (V)", &m_settings.stepVoltage,
0.0f, 10.0f);
sysid::CreateTooltip(
"This is the voltage that will be applied for the "
"dynamic voltage (acceleration) tests.");
// Create a section for tests.
ImGui::Separator();
ImGui::Spacing();
ImGui::Text("Tests");
auto CreateTest = [this, width](const char* text, const char* itext) {
// Display buttons if we have an NT connection.
if (nt::NetworkTableInstance::GetDefault().IsConnected()) {
// Create button to run tests.
if (ImGui::Button(text)) {
// Open the warning message.
ImGui::OpenPopup("Warning");
m_manager->BeginTest(itext);
m_opened = text;
}
if (m_opened == text && ImGui::BeginPopupModal("Warning")) {
ImGui::TextWrapped("%s", m_popupText.c_str());
if (ImGui::Button(m_manager->IsActive() ? "End Test" : "Close")) {
m_manager->EndTest();
ImGui::CloseCurrentPopup();
m_opened = "";
}
ImGui::EndPopup();
}
} else {
// Show disabled text when there is no connection.
ImGui::TextDisabled("%s", text);
}
// Show whether the tests were run or not.
bool run = m_manager->HasRunTest(itext);
ImGui::SameLine(width * 0.7);
ImGui::Text(run ? "Run" : "Not Run");
};
CreateTest("Quasistatic Forward", "slow-forward");
CreateTest("Quasistatic Backward", "slow-backward");
CreateTest("Dynamic Forward", "fast-forward");
CreateTest("Dynamic Backward", "fast-backward");
m_manager->RegisterDisplayCallback(
[this](const auto& str) { m_popupText = str; });
// Display the path to where the JSON will be saved and a button to select the
// location.
ImGui::Separator();
ImGui::Spacing();
ImGui::Text("Save Location");
if (ImGui::Button("Choose")) {
m_selector = std::make_unique<pfd::select_folder>("Select Folder");
}
ImGui::SameLine();
ImGui::InputText("##savelocation", &m_jsonLocation,
ImGuiInputTextFlags_ReadOnly);
// Add button to save.
ImGui::SameLine(width * 0.9);
if (ImGui::Button("Save")) {
try {
m_manager->SaveJSON(m_jsonLocation);
} catch (const std::exception& e) {
ImGui::OpenPopup("Exception Caught!");
m_exception = e.what();
}
}
// Handle exceptions.
if (ImGui::BeginPopupModal("Exception Caught!")) {
ImGui::Text("%s", m_exception.c_str());
if (ImGui::Button("Close")) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
// Run periodic methods.
SelectDataFolder();
m_ntSettings.Update();
m_manager->Update();
}
void Logger::SelectDataFolder() {
// If the selector exists and is ready with a result, we can store it.
if (m_selector && m_selector->ready()) {
m_jsonLocation = m_selector->result();
m_selector.reset();
}
}

76
sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
View File

@@ -17,6 +17,7 @@
#include <units/time.h>
#include <wpi/Logger.h>
#include <wpi/StringMap.h>
#include <wpi/json.h>
#include "sysid/analysis/AnalysisType.h"
@@ -33,6 +34,9 @@ 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<double> 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<int>(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<int>(m_settings.dataset)];
}
Storage& GetFilteredData() { return m_filteredDataset; }
/**
* Returns the original dataset.
*
* @return The original (untouched) dataset
*/
Storage& GetOriginalData() {
return m_originalDataset[static_cast<int>(m_settings.dataset)];
}
Storage& GetOriginalData() { return m_originalDataset; }
/**
* Returns the minimum duration of the Step Voltage Test of the currently
@@ -314,22 +295,14 @@ class AnalysisManager {
return m_startTimes;
}
bool HasData() const {
return !m_originalDataset[static_cast<int>(
Settings::DrivetrainDataset::kCombined)]
.empty();
}
bool HasData() const { return !m_originalDataset.empty(); }
private:
wpi::Logger& m_logger;
// This is used to store the various datasets (i.e. Combined, Forward,
// Backward, etc.)
wpi::json m_json;
std::array<Storage, 3> m_originalDataset;
std::array<Storage, 3> m_rawDataset;
std::array<Storage, 3> m_filteredDataset;
Storage m_originalDataset;
Storage m_rawDataset;
Storage m_filteredDataset;
// Stores the various start times of the different tests.
std::array<units::second_t, 4> m_startTimes;
@@ -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<double>::infinity()};
std::vector<units::second_t> m_positionDelays;
std::vector<units::second_t> m_velocityDelays;
// Stores an optional track width if we are doing the drivetrain angular test.
std::optional<double> m_trackWidth;
void PrepareGeneralData();
void PrepareAngularDrivetrainData();
void PrepareLinearDrivetrainData();
};
} // namespace sysid

2
sysid/src/main/native/include/sysid/analysis/AnalysisType.h
View File

@@ -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"};

3
sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
View File

@@ -98,6 +98,9 @@ double GetNoiseFloor(
const std::vector<PreparedData>& data, int window,
std::function<double(const PreparedData&)> accessorFunction);
double GetMaxSpeed(const std::vector<PreparedData>& data,
std::function<double(const PreparedData&)> accessorFunction);
/**
* Reduces noise in velocity data by applying a median filter.
*

24
sysid/src/main/native/include/sysid/analysis/JSONConverter.h
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <string>
#include <string_view>
#include <wpi/Logger.h>
namespace sysid {
/**
* Converts a JSON from the old frc-characterization format to the new sysid
* format.
*
* @param path The path to the old JSON.
* @param logger The logger instance for log messages.
* @return The full file path of the newly saved JSON.
*/
std::string ConvertJSON(std::string_view path, wpi::Logger& logger);
std::string ToCSV(std::string_view path, wpi::Logger& logger);
} // namespace sysid

34
sysid/src/main/native/include/sysid/analysis/Storage.h
View File

@@ -4,12 +4,46 @@
#pragma once
#include <string>
#include <vector>
#include <units/time.h>
#include <units/voltage.h>
#include <wpi/StringMap.h>
#include "sysid/analysis/AnalysisType.h"
namespace sysid {
struct MotorData {
// name of the *motor*, not the test
std::string name;
// Data for a single contiguous motor test
// Timestamps are not necessarily aligned!
struct Run {
template <typename T>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
struct Sample {
Sample(units::second_t time, T measurement)
: time{time}, measurement{measurement} {}
units::second_t time;
T measurement;
};
std::vector<Sample<units::volt_t>> voltage;
std::vector<Sample<double>> position;
std::vector<Sample<double>> velocity;
};
std::vector<Run> runs;
};
struct TestData {
std::string distanceUnit;
AnalysisType mechanismType;
wpi::StringMap<MotorData> motorData;
};
/**
* Represents each data point after it is cleaned and various parameters are
* calculated.

237
sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h
View File

@@ -1,237 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <array>
#include <cstddef>
#include <functional>
#include <memory>
#include <string>
#include <string_view>
#include <utility>
#include <vector>
#include <networktables/BooleanTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/IntegerTopic.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/StringTopic.h>
#include <units/time.h>
#include <wpi/Logger.h>
#include <wpi/SmallVector.h>
#include <wpi/json.h>
#include "sysid/analysis/AnalysisType.h"
namespace sysid {
/**
* This class is responsible for collecting data from the robot and storing it
* inside a JSON.
*/
class TelemetryManager {
public:
/**
* Represents settings for an instance of the TelemetryManager class. This
* contains information about the quasistatic ramp rate for slow tests, the
* step voltage for fast tests, and the mechanism type for characterization.
*/
struct Settings {
/**
* The rate at which the voltage should increase during the quasistatic test
* (V/s).
*/
double quasistaticRampRate = 0.25;
/**
* The voltage that the dynamic test should run at (V).
*/
double stepVoltage = 7.0;
/**
* The units the mechanism moves per recorded rotation. The sysid project
* will be recording things in rotations of the shaft so the
* unitsPerRotation is to convert those measurements to the units the user
* wants to use.
*/
double unitsPerRotation = 1.0;
/**
* The name of the units used.
* Valid units: "Meters", "Feet", "Inches", "Radians", "Degrees",
* "Rotations"
*/
std::string units = "Meters";
/**
* The type of mechanism that will be analyzed.
* Supported mechanisms: Drivetrain, Angular Drivetrain, Elevator, Arm,
* Simple motor.
*/
AnalysisType mechanism = analysis::kDrivetrain;
};
/**
* Constructs an instance of the telemetry manager with the provided settings
* and NT instance to collect data over.
*
* @param settings The settings for this instance of the telemetry manager.
* @param logger The logger instance to use for log data.
* @param instance The NT instance to collect data over. The default value of
* this parameter should suffice in production; it should only
* be changed during unit testing.
*/
explicit TelemetryManager(const Settings& settings, wpi::Logger& logger,
nt::NetworkTableInstance instance =
nt::NetworkTableInstance::GetDefault());
/**
* Begins a test with the given parameters.
*
* @param name The name of the test.
*/
void BeginTest(std::string_view name);
/**
* Ends the currently running test. If there is no test running, this is a
* no-op.
*/
void EndTest();
/**
* Updates the telemetry manager -- this adds a new autospeed entry and
* collects newest data from the robot. This must be called periodically by
* the user.
*/
void Update();
/**
* Registers a callback that's called by the TelemetryManager when there is a
* message to display to the user.
*
* @param callback Callback function that runs based off of the message
*/
void RegisterDisplayCallback(std::function<void(std::string_view)> callback) {
m_callbacks.emplace_back(std::move(callback));
}
/**
* Saves a JSON with the stored data at the given location.
*
* @param location The location to save the JSON at (this is the folder that
* should contain the saved JSON).
* @return The full file path of the saved JSON.
*/
std::string SaveJSON(std::string_view location);
/**
* Returns whether a test is currently running.
*
* @return Whether a test is currently running.
*/
bool IsActive() const { return m_isRunningTest; }
/**
* Returns whether the specified test is running or has run.
*
* @param name The test to check.
*
* @return Whether the specified test is running or has run.
*/
bool HasRunTest(std::string_view name) const {
return std::find(m_tests.cbegin(), m_tests.cend(), name) != m_tests.end();
}
/**
* Gets the size of the stored data.
*
* @return The size of the stored data
*/
size_t GetCurrentDataSize() const { return m_params.data.size(); }
private:
enum class State { WaitingForEnable, RunningTest, WaitingForData };
/**
* Stores information about a currently running test. This information
* includes whether the robot will be traveling quickly (dynamic) or slowly
* (quasistatic), the direction of movement, the start time of the test,
* whether the robot is enabled, the current speed of the robot, and the
* collected data.
*/
struct TestParameters {
bool fast = false;
bool forward = false;
bool rotate = false;
State state = State::WaitingForEnable;
double enableStart = 0.0;
double disableStart = 0.0;
bool enabled = false;
double speed = 0.0;
std::string raw;
std::vector<std::vector<double>> data{};
bool overflow = false;
bool mechError = false;
TestParameters() = default;
TestParameters(bool fast, bool forward, bool rotate, State state)
: fast{fast}, forward{forward}, rotate{rotate}, state{state} {}
};
// Settings for this instance.
const Settings& m_settings;
// Logger.
wpi::Logger& m_logger;
// Test parameters for the currently running test.
TestParameters m_params;
bool m_isRunningTest = false;
// A list of running or already run tests.
std::vector<std::string> m_tests;
// Stores the test data.
wpi::json m_data;
// Display callbacks.
wpi::SmallVector<std::function<void(std::string_view)>, 1> m_callbacks;
// NetworkTables instance and entries.
nt::NetworkTableInstance m_inst;
std::shared_ptr<nt::NetworkTable> table = m_inst.GetTable("SmartDashboard");
nt::DoublePublisher m_voltageCommand =
table->GetDoubleTopic("SysIdVoltageCommand").Publish();
nt::StringPublisher m_testType =
table->GetStringTopic("SysIdTestType").Publish();
nt::BooleanPublisher m_rotate =
table->GetBooleanTopic("SysIdRotate").Publish();
nt::StringPublisher m_mechanism =
table->GetStringTopic("SysIdTest").Publish();
nt::BooleanPublisher m_overflowPub =
table->GetBooleanTopic("SysIdOverflow").Publish();
nt::BooleanSubscriber m_overflowSub =
table->GetBooleanTopic("SysIdOverflow").Subscribe(false);
nt::BooleanPublisher m_mechErrorPub =
table->GetBooleanTopic("SysIdWrongMech").Publish();
nt::BooleanSubscriber m_mechErrorSub =
table->GetBooleanTopic("SysIdWrongMech").Subscribe(false);
nt::StringSubscriber m_telemetry =
table->GetStringTopic("SysIdTelemetry").Subscribe("");
nt::IntegerSubscriber m_fmsControlData =
m_inst.GetTable("FMSInfo")
->GetIntegerTopic("FMSControlData")
.Subscribe(0);
nt::DoublePublisher m_ackNumberPub =
table->GetDoubleTopic("SysIdAckNumber").Publish();
nt::DoubleSubscriber m_ackNumberSub =
table->GetDoubleTopic("SysIdAckNumber").Subscribe(0);
int m_ackNumber;
};
} // namespace sysid

26
sysid/src/main/native/include/sysid/view/Analyzer.h
View File

@@ -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<double> m_trackWidth;
// Units
int m_selectedOverrideUnit = 0;
double m_conversionFactor = 0.0;
// Data analysis
std::unique_ptr<AnalysisManager> m_manager;
int m_dataset = 0;
int m_window = 8;
double m_threshold = 0.2;
float m_stepTestDuration = 0.0;
float m_stepTestDuration = 10;
double m_gearingNumerator = 1.0;
double m_gearingDenominator = 1.0;
bool combinedGraphFit = false;
// File manipulation
std::unique_ptr<pfd::open_file> m_selector;
std::string m_location;
// Logger
wpi::Logger& m_logger;

80
sysid/src/main/native/include/sysid/view/DataSelector.h Normal file
View File

@@ -0,0 +1,80 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <functional>
#include <future>
#include <map>
#include <string>
#include <vector>
#include <glass/View.h>
#include <glass/support/DataLogReaderThread.h>
#include <wpi/StringMap.h>
#include "sysid/analysis/Storage.h"
namespace glass {
class DataLogReaderEntry;
class Storage;
} // namespace glass
namespace wpi {
class Logger;
} // namespace wpi
namespace sysid {
/**
* Helps with loading datalog files.
*/
class DataSelector : public glass::View {
public:
/**
* Creates a data selector widget
*
* @param logger The program logger
*/
explicit DataSelector(glass::Storage& storage, wpi::Logger& logger)
/*: m_logger{logger}*/ {}
/**
* Displays the log loader window.
*/
void Display() override;
/**
* Resets view. Must be called whenever the DataLogReader goes away, as this
* class keeps references to DataLogReaderEntry objects.
*/
void Reset();
/**
* Called when new test data is loaded.
*/
std::function<void(TestData)> testdata;
private:
// wpi::Logger& m_logger;
using Runs = std::vector<glass::DataLogReaderRange>;
using State = std::map<std::string, Runs, std::less<>>; // full name
using Tests = std::map<std::string, State, std::less<>>; // e.g. "dynamic"
std::future<Tests> m_testsFuture;
Tests m_tests;
std::string m_selectedTest;
const glass::DataLogReaderEntry* m_testStateEntry = nullptr;
const glass::DataLogReaderEntry* m_velocityEntry = nullptr;
const glass::DataLogReaderEntry* m_positionEntry = nullptr;
const glass::DataLogReaderEntry* m_voltageEntry = nullptr;
double m_velocityScale = 1.0;
double m_positionScale = 1.0;
int m_selectedUnit = 0;
int m_selectedAnalysis = 0;
std::future<TestData> m_testdataFuture;
std::vector<std::string> m_testdataStats;
static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry);
TestData BuildTestData();
};
} // namespace sysid

57
sysid/src/main/native/include/sysid/view/JSONConverter.h
View File

@@ -1,57 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <functional>
#include <memory>
#include <string>
#include <string_view>
#include <glass/View.h>
#include <portable-file-dialogs.h>
#include <wpi/Logger.h>
namespace sysid {
/**
* Helps with converting different JSONs into different formats. Primarily
* enables users to convert an old 2020 FRC-Characterization JSON into a SysId
* JSON or a SysId JSON into a CSV file.
*/
class JSONConverter {
public:
/**
* Creates a JSONConverter widget
*
* @param logger The program logger
*/
explicit JSONConverter(wpi::Logger& logger) : m_logger(logger) {}
/**
* Function to display the SysId JSON to CSV converter.
*/
void DisplayCSVConvert();
private:
/**
* Helper method to display a specific JSON converter
*
* @param tooltip The tooltip describing the JSON converter
* @param converter The function that takes a filename path and performs the
* previously specifid JSON conversion.
*/
void DisplayConverter(
const char* tooltip,
std::function<std::string(std::string_view, wpi::Logger&)> converter);
wpi::Logger& m_logger;
std::string m_location;
std::unique_ptr<pfd::open_file> m_opener;
std::string m_exception;
double m_timestamp = 0;
};
} // namespace sysid

78
sysid/src/main/native/include/sysid/view/LogLoader.h Normal file
View File

@@ -0,0 +1,78 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include <string>
#include <string_view>
#include <vector>
#include <glass/View.h>
#include <wpi/Signal.h>
namespace glass {
class DataLogReaderEntry;
class DataLogReaderThread;
class Storage;
} // namespace glass
namespace pfd {
class open_file;
} // namespace pfd
namespace wpi {
class Logger;
} // namespace wpi
namespace sysid {
/**
* Helps with loading datalog files.
*/
class LogLoader : public glass::View {
public:
/**
* Creates a log loader widget
*
* @param logger The program logger
*/
explicit LogLoader(glass::Storage& storage, wpi::Logger& logger);
~LogLoader() override;
/**
* Displays the log loader window.
*/
void Display() override;
/**
* Signal called when the current file is unloaded (invalidates any
* LogEntry*).
*/
wpi::sig::Signal<> unload;
private:
// wpi::Logger& m_logger;
std::string m_filename;
std::unique_ptr<pfd::open_file> m_opener;
std::unique_ptr<glass::DataLogReaderThread> m_reader;
std::string m_error;
std::string m_filter;
struct EntryTreeNode {
explicit EntryTreeNode(std::string_view name) : name{name} {}
std::string name; // name of just this node
std::string path; // full path if entry is nullptr
const glass::DataLogReaderEntry* entry = nullptr;
std::vector<EntryTreeNode> children; // children, sorted by name
};
std::vector<EntryTreeNode> m_entryTree;
void RebuildEntryTree();
void DisplayEntryTree(const std::vector<EntryTreeNode>& nodes);
};
} // namespace sysid

82
sysid/src/main/native/include/sysid/view/Logger.h
View File

@@ -1,82 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include <string>
#include <glass/DataSource.h>
#include <glass/View.h>
#include <glass/networktables/NetworkTablesSettings.h>
#include <portable-file-dialogs.h>
#include <wpi/Logger.h>
#include "sysid/telemetry/TelemetryManager.h"
namespace glass {
class Storage;
} // namespace glass
namespace sysid {
/**
* The logger GUI takes care of running the system idenfitication tests over
* NetworkTables and logging the data. This data is then stored in a JSON file
* which can be used for analysis.
*/
class Logger : public glass::View {
public:
/**
* Makes a logger widget.
*
* @param storage The glass storage object
* @param logger A logger object that keeps track of the program's logs
*/
Logger(glass::Storage& storage, wpi::Logger& logger);
/**
* Displays the logger widget.
*/
void Display() override;
/**
* The different mechanism / analysis types that are supported.
*/
static constexpr const char* kTypes[] = {"Drivetrain", "Drivetrain (Angular)",
"Arm", "Elevator", "Simple"};
/**
* The different units that are supported.
*/
static constexpr const char* kUnits[] = {"Meters", "Feet", "Inches",
"Radians", "Rotations", "Degrees"};
private:
/**
* Handles the logic of selecting a folder to save the SysId JSON to
*/
void SelectDataFolder();
wpi::Logger& m_logger;
TelemetryManager::Settings m_settings;
int m_selectedType = 0;
int m_selectedUnit = 0;
std::unique_ptr<TelemetryManager> m_manager =
std::make_unique<TelemetryManager>(m_settings, m_logger);
std::unique_ptr<pfd::select_folder> m_selector;
std::string m_jsonLocation;
glass::NetworkTablesSettings m_ntSettings;
bool m_isRotationalUnits = false;
std::string m_popupText;
std::string m_opened;
std::string m_exception;
};
} // namespace sysid

9
sysid/src/main/native/include/sysid/view/UILayout.h
View File

@@ -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 =

4
sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp
View File

@@ -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"));

48
sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
View File

@@ -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;

20
sysid/src/test/native/cpp/analysis/FilterTest.cpp
View File

@@ -45,19 +45,13 @@ TEST(FilterTest, NoiseFloor) {
TEST(FilterTest, StepTrim) {
std::vector<sysid::PreparedData> testData = {
{0_s, 1, 2, 3, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 0.25, 0},
{2_s, 1, 2, 3, 5_ms, 0.5, 0}, {3_s, 1, 2, 3, 5_ms, 0.45, 0},
{4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
{6_s, 1, 2, 3, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0},
{8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0},
{0_s, 1, 2, 0, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 0.25, 0},
{2_s, 1, 2, 0, 5_ms, 10, 0}, {3_s, 1, 2, 3, 5_ms, 0.45, 0},
{4_s, 1, 2, 9.6, 5_ms, 0, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0},
{6_s, 1, 2, 0, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0},
{8_s, 1, 2, 10, 5_ms, 0, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0},
};
std::vector<sysid::PreparedData> expectedData = {
{2_s, 1, 2, 3, 5_ms, 0.5, 0},
{3_s, 1, 2, 3, 5_ms, 0.45, 0},
{4_s, 1, 2, 3, 5_ms, 0.35, 0},
{5_s, 1, 2, 3, 5_ms, 0.15, 0}};
auto maxTime = 9_s;
auto minTime = maxTime;
@@ -66,9 +60,7 @@ 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());
}

2
wpiutil/src/main/native/include/wpi/DataLogReader.h
View File

@@ -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;