diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h index 23d9132f97f..8e323d0641e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h @@ -77,7 +77,7 @@ class Mechanism { * null. */ Mechanism(std::function drive, - std::function log, + std::function log, frc2::Subsystem* subsystem, const std::string& name) : m_drive(drive), m_log(log), m_subsystem(subsystem), m_name(name) {} @@ -97,14 +97,14 @@ class Mechanism { * title for the routine's test state, e.g. "sysid-test-state-subsystem". */ Mechanism(std::function drive, - std::function log, + std::function log, frc2::Subsystem* subsystem) : m_drive(drive), m_log(log), m_subsystem(subsystem), m_name(m_subsystem->GetName()) {} std::function m_drive; - std::function m_log; + std::function m_log; frc2::Subsystem* m_subsystem; const std::string m_name; }; diff --git a/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp b/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp index 162792aa67e..2e82d76d15e 100644 --- a/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp +++ b/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp @@ -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) { diff --git a/wpilibc/src/main/native/include/frc/sysid/MotorLog.h b/wpilibc/src/main/native/include/frc/sysid/MotorLog.h deleted file mode 100644 index af441dd14f9..00000000000 --- a/wpilibc/src/main/native/include/frc/sysid/MotorLog.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include - -#include -#include -#include -#include -#include - -namespace frc::sysid { -/** Handle for recording SysId motor data to WPILog. */ -class MotorLog { - public: - MotorLog() = default; - virtual ~MotorLog() {} - /** - * Records a frame of motor data during a SysId routine. Data fields are - * recorded as their own log entries (named like - * "voltage-motorName-mechanismName"), and have their units specified in - * metadata. Assumes a linear distance dimension, and records all data to - * WPILog in meters. - * - * @param voltage Voltage applied to the motor. - * @param distance Total linear displacement of the motor. - * @param velocity Rate of change of linear displacement of the motor. - * @param motorName Name of the motor, used as the topic name for the WPILog - * entry for this data frame. - */ - virtual void RecordFrameLinear(units::volt_t voltage, units::meter_t distance, - units::meters_per_second_t velocity, - const std::string& motorName) = 0; - /** - * Records a frame of motor data during a SysId routine. Data fields are - * recorded as their own log entries (named like - * "voltage-motorName-mechanismName"), and have their units specified in - * metadata. Assumes a linear distance dimension, and records all data to - * WPILog in rotations. - * - * @param voltage Voltage applied to the motor. - * @param angle Total angular displacement of the motor. - * @param velocity Rate of change of angular displacement of the motor. - * @param motorName Name of the motor, used as the topic name for the WPILog - * entry for this data frame. - */ - virtual void RecordFrameAngular(units::volt_t voltage, units::turn_t angle, - units::turns_per_second_t velocity, - const std::string& motorName) = 0; - - private: -}; -} // namespace frc::sysid diff --git a/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h b/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h index 8109125fd67..a44d6ec1d6b 100644 --- a/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h +++ b/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h @@ -8,8 +8,11 @@ #include #include - -#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 { @@ -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; + using LogEntries = wpi::StringMap; + using turns_per_second = units::compound_unit>; + using turns_per_second_t = units::unit_t; + using turns_per_second_squared_t = + units::unit_t>>; + using meters_per_second_squared_t = + units::unit_t>>; 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. * @@ -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 @@ -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> - m_logEntries{}; + LogEntries m_logEntries{}; std::string m_logName{}; wpi::log::StringLogEntry m_state{}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h index c2f51679c7b..8857a1a98da 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h @@ -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}, @@ -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}}; };