Skip to content

Commit

Permalink
get simple motor analysis working from wpilog
Browse files Browse the repository at this point in the history
  • Loading branch information
Oblarg committed Jan 4, 2024
1 parent cbe4105 commit ee5caa5
Show file tree
Hide file tree
Showing 12 changed files with 92 additions and 370 deletions.
7 changes: 6 additions & 1 deletion sysid/src/main/native/cpp/App.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,8 +104,13 @@ void Application(std::string_view saveDir) {

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

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));
Expand All @@ -114,7 +119,7 @@ void Application(std::string_view saveDir) {
gWindowManager->AddWindow("Data Selector", std::move(dataSelector));

gAnalyzerWindow = gWindowManager->AddWindow(
"Analyzer", std::make_unique<sysid::Analyzer>(storage, gLogger));
"Analyzer", std::move(analyzer));

gProgramLogWindow = gWindowManager->AddWindow(
"Program Log", std::make_unique<glass::LogView>(&gLog));
Expand Down
50 changes: 22 additions & 28 deletions sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,68 +125,62 @@ void AnalysisManager::PrepareGeneralData() {
for (auto& it : m_data.motorData) {
auto key = it.first();
preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
}


// Store the original datasets
m_originalDataset =
CombineDatasets(preparedData["original-raw-slow-forward"],
preparedData["original-raw-slow-backward"],
preparedData["original-raw-fast-forward"],
preparedData["original-raw-fast-backward"]);
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_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 =
CombineDatasets(
preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
preparedData["raw-quasistatic-forward"], preparedData["raw-quasistatic-reverse"],
preparedData["raw-dynamic-forward"], preparedData["raw-dynamic-reverse"]);

// Store the filtered datasets
m_filteredDataset =
CombineDatasets(
preparedData["slow-forward"], preparedData["slow-backward"],
preparedData["fast-forward"], preparedData["fast-backward"]);
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};
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} {}

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

WPI_INFO(m_logger, "Read {}", path);
}
: 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_data.mechanismType.name);
// WPI_INFO(m_logger, "Preparing {} data", m_data.mechanismType.name);

PrepareGeneralData();

Expand All @@ -201,8 +195,8 @@ AnalysisManager::FeedforwardGains AnalysisManager::CalculateFeedforward() {

WPI_INFO(m_logger, "{}", "Calculating Gains");
// Calculate feedforward gains from the data.
const auto& ff = sysid::CalculateFeedforwardGains(GetFilteredData(), m_data.mechanismType);
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];
Expand Down
6 changes: 0 additions & 6 deletions sysid/src/main/native/cpp/analysis/AnalysisType.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
52 changes: 28 additions & 24 deletions sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,26 +153,19 @@ 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;
});

if (endIt != data->rend()) {
// Calculate default duration
settings->stepTestDuration = std::min(
endIt->timestamp - data->front().timestamp + minStepTime + 1_s,
maxStepTime);
} else {
settings->stepTestDuration = maxStepTime;
}
// Find maximum speed reached
const auto maxSpeed = GetMaxSpeed(
*data, [](auto&& pt) { return pt.velocity; });
// Find place where 90% of maximum speed exceeded
auto endIt = std::find_if(
data->begin(), data->end(), [&](const PreparedData& entry) {
return std::abs(entry.velocity) > maxSpeed * 0.9;
});

if (endIt != data->end()) {
settings->stepTestDuration = std::min(
endIt->timestamp - data->front().timestamp + minStepTime,
maxStepTime);
}

// Find first entry greater than the step test duration
Expand All @@ -186,6 +179,7 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
if (maxIt != data->end()) {
data->erase(maxIt, data->end());
}

return std::make_tuple(minStepTime, positionDelay, velocityDelay);
}

Expand All @@ -204,6 +198,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;

Expand Down Expand Up @@ -301,7 +305,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;
Expand All @@ -327,7 +331,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,
Expand All @@ -342,7 +346,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 ||
Expand All @@ -366,7 +370,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-");

Expand Down
Loading

0 comments on commit ee5caa5

Please sign in to comment.