Skip to content

Commit

Permalink
improve flexibility of motor logging c++
Browse files Browse the repository at this point in the history
  • Loading branch information
Oblarg committed Jan 5, 2024
1 parent ce01694 commit caa934e
Show file tree
Hide file tree
Showing 5 changed files with 85 additions and 131 deletions.
6 changes: 3 additions & 3 deletions wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ class Mechanism {
* null.
*/
Mechanism(std::function<void(units::volt_t)> drive,
std::function<void(frc::sysid::MotorLog*)> log,
std::function<void(frc::sysid::SysIdRoutineLog*)> log,
frc2::Subsystem* subsystem, const std::string& name)
: m_drive(drive), m_log(log), m_subsystem(subsystem), m_name(name) {}

Expand All @@ -97,14 +97,14 @@ class Mechanism {
* title for the routine's test state, e.g. "sysid-test-state-subsystem".
*/
Mechanism(std::function<void(units::volt_t)> drive,
std::function<void(frc::sysid::MotorLog*)> log,
std::function<void(frc::sysid::SysIdRoutineLog*)> log,
frc2::Subsystem* subsystem)
: m_drive(drive),
m_log(log),
m_subsystem(subsystem),
m_name(m_subsystem->GetName()) {}
std::function<void(units::volt_t)> m_drive;
std::function<void(frc::sysid::MotorLog*)> m_log;
std::function<void(frc::sysid::SysIdRoutineLog*)> m_log;
frc2::Subsystem* m_subsystem;
const std::string m_name;
};
Expand Down
56 changes: 14 additions & 42 deletions wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,54 +15,26 @@ SysIdRoutineLog::SysIdRoutineLog(const std::string& logName)
m_state.Append(StateEnumToString(State::kNone));
}

void SysIdRoutineLog::RecordFrameLinear(units::volt_t voltage,
units::meter_t distance,
units::meters_per_second_t velocity,
const std::string& motorName) {
if (!m_logEntries.contains(motorName)) {
wpi::log::DataLog& log = frc::DataLogManager::GetLog();

m_logEntries[motorName].emplace(
"voltage", wpi::log::DoubleLogEntry(
log, "voltage-" + motorName + "-" + m_logName, "Volt"));
m_logEntries[motorName].emplace(
"distance",
wpi::log::DoubleLogEntry(log, "distance-" + motorName + "-" + m_logName,
"meters"));
m_logEntries[motorName].emplace(
"velocity",
wpi::log::DoubleLogEntry(log, "velocity-" + motorName + "-" + m_logName,
"meters-per-second"));
}

m_logEntries[motorName]["voltage"].Append(voltage.value());
m_logEntries[motorName]["distance"].Append(distance.value());
m_logEntries[motorName]["velocity"].Append(velocity.value());
SysIdRoutineLog::MotorLog::MotorLog(const std::string& motorName, const std::string& logName, LogEntries* logEntries)
: m_motorName(motorName), m_logName(logName), m_logEntries(logEntries) {
(*logEntries)[motorName] = MotorEntries();
}

void SysIdRoutineLog::RecordFrameAngular(units::volt_t voltage,
units::turn_t angle,
units::turns_per_second_t velocity,
const std::string& motorName) {
if (!m_logEntries.contains(motorName)) {
SysIdRoutineLog::MotorLog& SysIdRoutineLog::MotorLog::value(const std::string& name, double value, const std::string& unit) {
auto& motorEntries = (*m_logEntries)[m_motorName];

if (!motorEntries.contains(name)) {
wpi::log::DataLog& log = frc::DataLogManager::GetLog();

m_logEntries[motorName].emplace(
"voltage", wpi::log::DoubleLogEntry(
log, "voltage-" + motorName + "-" + m_logName, "Volt"));
m_logEntries[motorName].emplace(
"distance",
wpi::log::DoubleLogEntry(log, "distance-" + motorName + "-" + m_logName,
"rotations"));
m_logEntries[motorName].emplace(
"velocity",
wpi::log::DoubleLogEntry(log, "velocity-" + motorName + "-" + m_logName,
"rotations-per-second"));
motorEntries[name] = wpi::log::DoubleLogEntry(log, name + "-" + m_motorName + "-" + m_logName, unit);
}

m_logEntries[motorName]["voltage"].Append(voltage.value());
m_logEntries[motorName]["distance"].Append(angle.value());
m_logEntries[motorName]["velocity"].Append(velocity.value());
motorEntries[name].Append(value);
return *this;
}

SysIdRoutineLog::MotorLog SysIdRoutineLog::Motor(const std::string& motorName) {
return MotorLog{motorName, m_logName, &m_logEntries};
}

void SysIdRoutineLog::RecordState(State state) {
Expand Down
56 changes: 0 additions & 56 deletions wpilibc/src/main/native/include/frc/sysid/MotorLog.h
View file

This file was deleted.

72 changes: 60 additions & 12 deletions wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,11 @@
#include <unordered_map>

#include <wpi/DataLog.h>

#include "frc/sysid/MotorLog.h"
#include "units/voltage.h"
#include "units/current.h"
#include "units/length.h"
#include "units/angle.h"
#include "units/velocity.h"

namespace frc::sysid {

Expand All @@ -27,8 +30,58 @@ enum class State {
* (quasistatic and dynamic, forward and reverse) should have its own
* SysIdRoutineLog instance, with a unique log name.
*/
class SysIdRoutineLog : public MotorLog {
class SysIdRoutineLog {
using MotorEntries = wpi::StringMap<wpi::log::DoubleLogEntry>;
using LogEntries = wpi::StringMap<MotorEntries>;
using turns_per_second = units::compound_unit<units::turns, units::inverse<units::seconds>>;
using turns_per_second_t = units::unit_t<turns_per_second>;
using turns_per_second_squared_t =
units::unit_t<units::compound_unit<turns_per_second, units::inverse<units::seconds>>>;
using meters_per_second_squared_t =
units::unit_t<units::compound_unit<units::meters_per_second, units::inverse<units::seconds>>>;
public:
class MotorLog {
public:
MotorLog(const std::string& motorName, const std::string& logName, LogEntries* logEntries);

MotorLog& value(const std::string& name, double value, const std::string& unit);

MotorLog& voltage(units::volt_t voltage) {
return value("voltage", voltage.value(), voltage.name());
};

MotorLog& position(units::meter_t position) {
return value("position", position.value(), position.name());
};

MotorLog& position(units::turn_t position) {
return value("position", position.value(), position.name());
};

MotorLog& velocity(units::meters_per_second_t velocity) {
return value("velocity", velocity.value(), velocity.name());
};

MotorLog& velocity(turns_per_second_t velocity) {
return value("velocity", velocity.value(), velocity.name());
};

MotorLog& acceleration(meters_per_second_squared_t acceleration) {
return value("acceleration", acceleration.value(), acceleration.name());
}

MotorLog& acceleration(turns_per_second_squared_t acceleration) {
return value("acceleration", acceleration.value(), acceleration.name());
}

MotorLog& current(units::ampere_t current) {
return value("current", current.value(), current.name());
};
private:
std::string m_motorName;
std::string m_logName;
LogEntries* m_logEntries;
};
/**
* Create a new logging utility for a SysId test routine.
*
Expand All @@ -38,12 +91,6 @@ class SysIdRoutineLog : public MotorLog {
* appear in WPILog under the "sysid-test-state-logName" entry.
*/
explicit SysIdRoutineLog(const std::string& logName);
void RecordFrameLinear(units::volt_t voltage, units::meter_t distance,
units::meters_per_second_t velocity,
const std::string& motorName) override;
void RecordFrameAngular(units::volt_t voltage, units::turn_t angle,
units::turns_per_second_t velocity,
const std::string& motorName) override;
/**
* Records the current state of the SysId test routine. Should be called once
* per iteration during tests with the type of the current test, and once upon
Expand All @@ -52,12 +99,13 @@ class SysIdRoutineLog : public MotorLog {
* @param state The current state of the SysId test routine.
*/
void RecordState(State state);

MotorLog Motor(const std::string& motorName);

static std::string StateEnumToString(State state);

private:
std::unordered_map<std::string,
std::unordered_map<std::string, wpi::log::DoubleLogEntry>>
m_logEntries{};
LogEntries m_logEntries{};
std::string m_logName{};
wpi::log::StringLogEntry m_state{};
};
Expand Down
26 changes: 8 additions & 18 deletions wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,6 @@ class Drive : public frc2::SubsystemBase {
constants::drive::kRightEncoderPorts[1],
constants::drive::kRightEncoderReversed};

units::volt_t m_appliedVoltage{0_V};
units::meter_t m_distance{0_m};
units::meters_per_second_t m_velocity{0_mps};

frc2::sysid::SysIdRoutine m_sysIdRoutine{
frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
std::nullopt},
Expand All @@ -51,20 +47,14 @@ class Drive : public frc2::SubsystemBase {
m_rightMotor.SetVoltage(driveVoltage);
},
[this](frc::sysid::MotorLog* log) {
log->RecordFrameLinear(
m_appliedVoltage = m_leftMotor.Get() *
frc::RobotController::GetBatteryVoltage(),
m_distance = units::meter_t{m_leftEncoder.GetDistance()},
m_velocity =
units::meters_per_second_t{m_leftEncoder.GetRate()},
"drive-left");
log->RecordFrameLinear(
m_appliedVoltage = m_rightMotor.Get() *
frc::RobotController::GetBatteryVoltage(),
m_distance = units::meter_t{m_rightEncoder.GetDistance()},
m_velocity =
units::meters_per_second_t{m_rightEncoder.GetRate()},
"drive-right");
log->Motor("drive-left")
.voltage(m_leftMotor.Get() * frc::RobotController::GetBatteryVoltage())
.position(units::meter_t{m_leftEncoder.GetDistance()})
.velocity(units::meters_per_second_t{m_leftEncoder.GetRate()});
log->Motor("drive-right")
.voltage(m_rightMotor.Get() * frc::RobotController::GetBatteryVoltage())
.position(units::meter_t{m_rightEncoder.GetDistance()})
.velocity(units::meters_per_second_t{m_rightEncoder.GetRate()});
},
this}};
};

0 comments on commit caa934e

Please sign in to comment.