diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java index 0e4541e1ca..6a049a6021 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java @@ -151,10 +151,11 @@ public class SysIdRoutine extends SysIdRoutineLog { * * @param drive Sends the SysId-specified drive signal to the mechanism motors during test * routines. - * @param log Returns measured data (voltages, positions, velocities) of the mechanism motors - * during test routines. To return data, call `recordFrame` on the supplied - * `SysIdRoutineLog` instance. Multiple motors can return data within a single `log` - * callback by calling `recordFrame` multiple times. + * @param log Returns measured data of the mechanism motors during test routines. To return + * data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then + * call one or more of the chainable logging handles (e.g. `voltage`) on the returned + * `MotorLog`. Multiple motors can be logged in a single callback by calling `motor` + * multiple times. * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized. * Will be declared as a requirement for the returned test commands. * @param name The name of the mechanism being tested. Will be appended to the log entry title @@ -178,10 +179,11 @@ public class SysIdRoutine extends SysIdRoutineLog { * * @param drive Sends the SysId-specified drive signal to the mechanism motors during test * routines. - * @param log Returns measured data (voltages, positions, velocities) of the mechanism motors - * during test routines. To return data, call `recordFrame` on the supplied - * `SysIdRoutineLog` instance. Multiple motors can return data within a single `log` - * callback by calling `recordFrame` multiple times. + * @param log Returns measured data of the mechanism motors during test routines. To return + * data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then + * call one or more of the chainable logging handles (e.g. `voltage`) on the returned + * `MotorLog`. Multiple motors can be logged in a single callback by calling `motor` + * multiple times. * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized. * Will be declared as a requirement for the returned test commands. The subsystem's `name` * will be appended to the log entry title for the routine's test state, e.g. 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 8b90cdc6ba..021059f11a 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h @@ -1,196 +1,200 @@ -// 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 - -#include "frc2/command/CommandPtr.h" -#include "frc2/command/Subsystem.h" - -namespace frc2::sysid { - -using ramp_rate_t = units::unit_t< - units::compound_unit>>; - -/** Hardware-independent configuration for a SysId test routine. */ -class Config { - public: - /// The voltage ramp rate used for quasistatic test routines. - ramp_rate_t m_rampRate{1_V / 1_s}; - - /// The step voltage output used for dynamic test routines. - units::volt_t m_stepVoltage{7_V}; - - /// Safety timeout for the test routine commands. - units::second_t m_timeout{10_s}; - - /// Optional handle for recording test state in a third-party logging - /// solution. - std::function m_recordState; - - /** - * Create a new configuration for a SysId test routine. - * - * @param rampRate The voltage ramp rate used for quasistatic test routines. - * Defaults to 1 volt per second if left null. - * @param stepVoltage The step voltage output used for dynamic test routines. - * Defaults to 7 volts if left null. - * @param timeout Safety timeout for the test routine commands. Defaults to 10 - * seconds if left null. - * @param recordState Optional handle for recording test state in a - * third-party logging solution. If provided, the test routine state will be - * passed to this callback instead of logged in WPILog. - */ - Config(std::optional rampRate, - std::optional stepVoltage, - std::optional timeout, - std::optional> recordState) { - if (rampRate) { - m_rampRate = rampRate.value(); - } - if (stepVoltage) { - m_stepVoltage = stepVoltage.value(); - } - if (timeout) { - m_timeout = timeout.value(); - } - if (recordState) { - m_recordState = recordState.value(); - } - } -}; - -class Mechanism { - public: - /// Sends the SysId-specified drive signal to the mechanism motors during test - /// routines. - std::function m_drive; - - /// Returns measured data (voltages, positions, velocities) of the mechanism - /// motors during test routines. - std::function m_log; - - /// The subsystem containing the motor(s) that is (or are) being - /// characterized. - frc2::Subsystem* m_subsystem; - - /// The name of the mechanism being tested. Will be appended to the log entry - /// title for the routine's test state, e.g. "sysid-test-state-mechanism". - std::string m_name; - - /** - * Create a new mechanism specification for a SysId routine. - * - * @param drive Sends the SysId-specified drive signal to the mechanism motors - * during test routines. - * @param log Returns measured data (voltages, positions, velocities) of the - * mechanism motors during test routines. To return data, call `RecordFrame` - * on the supplied `MotorLog` instance. Multiple motors can return data - * within a single `log` callback by calling `RecordFrame` multiple times. - * @param subsystem The subsystem containing the motor(s) that is (or are) - * being characterized. Will be declared as a requirement for the returned - * test commands. - * @param name The name of the mechanism being tested. Will be appended to the - * log entry * title for the routine's test state, e.g. - * "sysid-test-state-mechanism". Defaults to the name of the subsystem if - * left null. - */ - Mechanism(std::function drive, - std::function log, - frc2::Subsystem* subsystem, std::string_view name) - : m_drive{std::move(drive)}, - m_log{std::move(log)}, - m_subsystem{subsystem}, - m_name{name} {} - - /** - * Create a new mechanism specification for a SysId routine. Defaults the - * mechanism name to the subsystem name. - * - * @param drive Sends the SysId-specified drive signal to the mechanism motors - * during test routines. - * @param log Returns measured data (voltages, positions, velocities) of the - * mechanism motors during test routines. To return data, call `recordFrame` - * on the supplied `MotorLog` instance. Multiple motors can return data - * within a single `log` callback by calling `recordFrame` multiple times. - * @param subsystem The subsystem containing the motor(s) that is (or are) - * being characterized. Will be declared as a requirement for the returned - * test commands. The subsystem's `name` will be appended to the log entry - * title for the routine's test state, e.g. "sysid-test-state-subsystem". - */ - Mechanism(std::function drive, - std::function log, - frc2::Subsystem* subsystem) - : m_drive{std::move(drive)}, - m_log{std::move(log)}, - m_subsystem{subsystem}, - m_name{m_subsystem->GetName()} {} -}; - -/** - * Motor direction for a SysId test. - */ -enum Direction { - /// Forward. - kForward, - /// Reverse. - kReverse -}; - -/** - * A SysId characterization routine for a single mechanism. Mechanisms may have - * multiple motors. - * - * A single subsystem may have multiple mechanisms, but mechanisms should not - * share test routines. Each complete test of a mechanism should have its own - * SysIdRoutine instance, since the log name of the recorded data is determined - * by the mechanism name. - * - * The test state (e.g. "quasistatic-forward") is logged once per iteration - * during test execution, and once with state "none" when a test ends. Motor - * frames are logged every iteration during test execution. - * - * Timestamps are not coordinated across data, so motor frames and test state - * tags may be recorded on different log frames. Because frame alignment is not - * guaranteed, SysId parses the log by using the test state flag to determine - * the timestamp range for each section of the test, and then extracts the motor - * frames within the valid timestamp ranges. If a given test was run multiple - * times in a single logfile, the user will need to select which of the tests to - * use for the fit in the analysis tool. - */ -class SysIdRoutine : public frc::sysid::SysIdRoutineLog { - public: - /** - * Create a new SysId characterization routine. - * - * @param config Hardware-independent parameters for the SysId routine. - * @param mechanism Hardware interface for the SysId routine. - */ - SysIdRoutine(Config config, Mechanism mechanism) - : SysIdRoutineLog(mechanism.m_subsystem->GetName()), - m_config(config), - m_mechanism(mechanism), - m_recordState(config.m_recordState ? config.m_recordState - : [this](frc::sysid::State state) { - this->RecordState(state); - }) {} - - frc2::CommandPtr Quasistatic(Direction direction); - frc2::CommandPtr Dynamic(Direction direction); - - private: - Config m_config; - Mechanism m_mechanism; - units::volt_t m_outputVolts{0}; - std::function m_recordState; - frc::Timer timer; -}; -} // namespace frc2::sysid +// 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 + +#include "frc2/command/CommandPtr.h" +#include "frc2/command/Subsystem.h" + +namespace frc2::sysid { + +using ramp_rate_t = units::unit_t< + units::compound_unit>>; + +/** Hardware-independent configuration for a SysId test routine. */ +class Config { + public: + /// The voltage ramp rate used for quasistatic test routines. + ramp_rate_t m_rampRate{1_V / 1_s}; + + /// The step voltage output used for dynamic test routines. + units::volt_t m_stepVoltage{7_V}; + + /// Safety timeout for the test routine commands. + units::second_t m_timeout{10_s}; + + /// Optional handle for recording test state in a third-party logging + /// solution. + std::function m_recordState; + + /** + * Create a new configuration for a SysId test routine. + * + * @param rampRate The voltage ramp rate used for quasistatic test routines. + * Defaults to 1 volt per second if left null. + * @param stepVoltage The step voltage output used for dynamic test routines. + * Defaults to 7 volts if left null. + * @param timeout Safety timeout for the test routine commands. Defaults to 10 + * seconds if left null. + * @param recordState Optional handle for recording test state in a + * third-party logging solution. If provided, the test routine state will be + * passed to this callback instead of logged in WPILog. + */ + Config(std::optional rampRate, + std::optional stepVoltage, + std::optional timeout, + std::optional> recordState) { + if (rampRate) { + m_rampRate = rampRate.value(); + } + if (stepVoltage) { + m_stepVoltage = stepVoltage.value(); + } + if (timeout) { + m_timeout = timeout.value(); + } + if (recordState) { + m_recordState = recordState.value(); + } + } +}; + +class Mechanism { + public: + /// Sends the SysId-specified drive signal to the mechanism motors during test + /// routines. + std::function m_drive; + + /// Returns measured data (voltages, positions, velocities) of the mechanism + /// motors during test routines. + std::function m_log; + + /// The subsystem containing the motor(s) that is (or are) being + /// characterized. + frc2::Subsystem* m_subsystem; + + /// The name of the mechanism being tested. Will be appended to the log entry + /// title for the routine's test state, e.g. "sysid-test-state-mechanism". + std::string m_name; + + /** + * Create a new mechanism specification for a SysId routine. + * + * @param drive Sends the SysId-specified drive signal to the mechanism motors + * during test routines. + * @param log Returns measured data of the mechanism motors during test + * routines. To return data, call `Motor(string motorName)` on the supplied + * `SysIdRoutineLog` instance, and then call one or more of the chainable + * logging handles (e.g. `voltage`) on the returned `MotorLog`. Multiple + * motors can be logged in a single callback by calling `Motor` multiple + * times. + * @param subsystem The subsystem containing the motor(s) that is (or are) + * being characterized. Will be declared as a requirement for the returned + * test commands. + * @param name The name of the mechanism being tested. Will be appended to the + * log entry * title for the routine's test state, e.g. + * "sysid-test-state-mechanism". Defaults to the name of the subsystem if + * left null. + */ + Mechanism(std::function drive, + std::function log, + frc2::Subsystem* subsystem, std::string_view name) + : m_drive{std::move(drive)}, + m_log{std::move(log)}, + m_subsystem{subsystem}, + m_name{name} {} + + /** + * Create a new mechanism specification for a SysId routine. Defaults the + * mechanism name to the subsystem name. + * + * @param drive Sends the SysId-specified drive signal to the mechanism motors + * during test routines. + * @param log Returns measured data of the mechanism motors during test + * routines. To return data, call `Motor(string motorName)` on the supplied + * `SysIdRoutineLog` instance, and then call one or more of the chainable + * logging handles (e.g. `voltage`) on the returned `MotorLog`. Multiple + * motors can be logged in a single callback by calling `Motor` multiple + * times. + * @param subsystem The subsystem containing the motor(s) that is (or are) + * being characterized. Will be declared as a requirement for the returned + * test commands. The subsystem's `name` will be appended to the log entry + * title for the routine's test state, e.g. "sysid-test-state-subsystem". + */ + Mechanism(std::function drive, + std::function log, + frc2::Subsystem* subsystem) + : m_drive{std::move(drive)}, + m_log{std::move(log)}, + m_subsystem{subsystem}, + m_name{m_subsystem->GetName()} {} +}; + +/** + * Motor direction for a SysId test. + */ +enum Direction { + /// Forward. + kForward, + /// Reverse. + kReverse +}; + +/** + * A SysId characterization routine for a single mechanism. Mechanisms may have + * multiple motors. + * + * A single subsystem may have multiple mechanisms, but mechanisms should not + * share test routines. Each complete test of a mechanism should have its own + * SysIdRoutine instance, since the log name of the recorded data is determined + * by the mechanism name. + * + * The test state (e.g. "quasistatic-forward") is logged once per iteration + * during test execution, and once with state "none" when a test ends. Motor + * frames are logged every iteration during test execution. + * + * Timestamps are not coordinated across data, so motor frames and test state + * tags may be recorded on different log frames. Because frame alignment is not + * guaranteed, SysId parses the log by using the test state flag to determine + * the timestamp range for each section of the test, and then extracts the motor + * frames within the valid timestamp ranges. If a given test was run multiple + * times in a single logfile, the user will need to select which of the tests to + * use for the fit in the analysis tool. + */ +class SysIdRoutine : public frc::sysid::SysIdRoutineLog { + public: + /** + * Create a new SysId characterization routine. + * + * @param config Hardware-independent parameters for the SysId routine. + * @param mechanism Hardware interface for the SysId routine. + */ + SysIdRoutine(Config config, Mechanism mechanism) + : SysIdRoutineLog(mechanism.m_subsystem->GetName()), + m_config(config), + m_mechanism(mechanism), + m_recordState(config.m_recordState ? config.m_recordState + : [this](frc::sysid::State state) { + this->RecordState(state); + }) {} + + frc2::CommandPtr Quasistatic(Direction direction); + frc2::CommandPtr Dynamic(Direction direction); + + private: + Config m_config; + Mechanism m_mechanism; + units::volt_t m_outputVolts{0}; + std::function m_recordState; + frc::Timer timer; +}; +} // namespace frc2::sysid diff --git a/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h b/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h index beaa8bf58b..7f106bd173 100644 --- a/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h +++ b/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h @@ -46,46 +46,112 @@ class SysIdRoutineLog { using LogEntries = wpi::StringMap; public: + /** Logs data from a single motor during a SysIdRoutine. */ class MotorLog { public: - MotorLog(std::string_view motorName, std::string_view logName, - LogEntries* logEntries); - + /** + * Log a generic data value from the motor. + * + * @param name The name of the data field being recorded. + * @param value The numeric value of the data field. + * @param unit The unit string of the data field. + * @return The motor log (for call chaining). + */ MotorLog& value(std::string_view name, double value, std::string_view unit); + /** + * Log the voltage applied to the motor. + * + * @param voltage The voltage to record. + * @return The motor log (for call chaining). + */ MotorLog& voltage(units::volt_t voltage) { return value("voltage", voltage.value(), voltage.name()); } + /** + * Log the linear position of the motor. + * + * @param position The linear position to record. + * @return The motor log (for call chaining). + */ MotorLog& position(units::meter_t position) { return value("position", position.value(), position.name()); } + /** + * Log the angular position of the motor. + * + * @param position The angular position to record. + * @return The motor log (for call chaining). + */ MotorLog& position(units::turn_t position) { return value("position", position.value(), position.name()); } + /** + * Log the linear velocity of the motor. + * + * @param velocity The linear velocity to record. + * @return The motor log (for call chaining). + */ MotorLog& velocity(units::meters_per_second_t velocity) { return value("velocity", velocity.value(), velocity.name()); } + /** + * Log the angular velocity of the motor. + * + * @param velocity The angular velocity to record. + * @return The motor log (for call chaining). + */ MotorLog& velocity(units::turns_per_second_t velocity) { return value("velocity", velocity.value(), velocity.name()); } + /** + * Log the linear acceleration of the motor. + * + * @param acceleration The linear acceleration to record. + * @return The motor log (for call chaining). + */ MotorLog& acceleration(units::meters_per_second_squared_t acceleration) { return value("acceleration", acceleration.value(), acceleration.name()); } + /** + * Log the angular acceleration of the motor. + * + * @param acceleration The angular acceleration to record. + * @return The motor log (for call chaining). + */ MotorLog& acceleration(units::turns_per_second_squared_t acceleration) { return value("acceleration", acceleration.value(), acceleration.name()); } + /** + * Log the current applied to the motor. + * + * @param current The current to record. + * @return The motor log (for call chaining). + */ MotorLog& current(units::ampere_t current) { return value("current", current.value(), current.name()); } private: + friend class SysIdRoutineLog; + /** + * Create a new SysId motor log handle. + * + * @param motorName The name of the motor whose data is being logged. + * @param logName The name of the SysIdRoutineLog that this motor belongs + * to. + * @param logEntries The DataLog entries of the SysIdRoutineLog that this + * motor belongs to. + */ + MotorLog(std::string_view motorName, std::string_view logName, + LogEntries* logEntries); std::string m_motorName; std::string m_logName; LogEntries* m_logEntries; @@ -110,6 +176,12 @@ class SysIdRoutineLog { */ void RecordState(State state); + /** + * Log data from a motor during a SysId routine. + * + * @param motorName The name of the motor. + * @return Handle with chainable callbacks to log individual data fields. + */ MotorLog Motor(std::string_view motorName); static std::string StateEnumToString(State state); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java index a35cf75841..38bc46f400 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java @@ -73,7 +73,7 @@ public class SysIdRoutineLog { } } - /** Allows logging of data for a single motor during a SysIdRoutine. */ + /** Logs data from a single motor during a SysIdRoutine. */ public class MotorLog { private final String m_motorName; @@ -88,7 +88,7 @@ public class SysIdRoutineLog { } /** - * Record a generic data value from this motor. + * Log a generic data value from the motor. * * @param name The name of the data field being recorded. * @param value The numeric value of the data field. @@ -111,7 +111,7 @@ public class SysIdRoutineLog { } /** - * Record a voltage value from this motor. + * Log the voltage applied to the motor. * * @param voltage The voltage to record. * @return The motor log (for call chaining). @@ -121,7 +121,7 @@ public class SysIdRoutineLog { } /** - * Record a linear position value from this motor. + * Log the linear position of the motor. * * @param position The linear position to record. * @return The motor log (for call chaining). @@ -131,7 +131,7 @@ public class SysIdRoutineLog { } /** - * Record an angular position value from this motor. + * Log the angular position of the motor. * * @param position The angular position to record. * @return The motor log (for call chaining). @@ -141,7 +141,7 @@ public class SysIdRoutineLog { } /** - * Record a linear velocity value from this motor. + * Log the linear velocity of the motor. * * @param velocity The linear velocity to record. * @return The motor log (for call chaining). @@ -151,7 +151,7 @@ public class SysIdRoutineLog { } /** - * Record an angular velocity value from this motor. + * Log the angular velocity of the motor. * * @param velocity The angular velocity to record. * @return The motor log (for call chaining). @@ -161,7 +161,7 @@ public class SysIdRoutineLog { } /** - * Record a linear acceleration value from this motor. + * Log the linear acceleration of the motor. * * @param acceleration The linear acceleration to record. * @return The motor log (for call chaining). @@ -174,7 +174,7 @@ public class SysIdRoutineLog { } /** - * Record an angular acceleration value from this motor. + * Log the angular acceleration of the motor. * * @param acceleration The angular acceleration to record. * @return The motor log (for call chaining). @@ -187,7 +187,7 @@ public class SysIdRoutineLog { } /** - * Record a current value for this motor. + * Log the current applied to the motor. * * @param current The current to record. * @return The motor log (for call chaining). @@ -199,10 +199,10 @@ public class SysIdRoutineLog { } /** - * Returns the logging handle for a given motor. + * Log data from a motor during a SysId routine. * - * @param motorName The name of the motor to log data from. - * @return The logging handle for the specified motor. + * @param motorName The name of the motor. + * @return Handle with chainable callbacks to log individual data fields. */ public MotorLog motor(String motorName) { return new MotorLog(motorName);