From ca7373387c1fd53bda54e8d3e007b4db713aa029 Mon Sep 17 00:00:00 2001 From: Nolan T <97766806+WowMuchDoge@users.noreply.github.com> Date: Wed, 10 Jul 2024 16:46:57 -0500 Subject: [PATCH] Characterization (#176) * Fix characterization * Ad newline in SysIdDynamic function definition Co-authored-by: Evan Rust * Remove semicolon in function definition Co-authored-by: Evan Rust * Add newlines to characterization constants Co-authored-by: Evan Rust * Remove unnecessary comments --------- Co-authored-by: 5690 <5690Programmers@gmail.com> Co-authored-by: Evan Rust --- src/main/cpp/RobotContainer.cpp | 15 +++++- src/main/cpp/subsystems/DriveSubsystem.cpp | 49 +++++++++++++++++-- src/main/cpp/subsystems/MAXSwerveModule.cpp | 29 +++++++++++ src/main/include/Constants.h | 3 ++ .../constants/CharacterizationConstants.h | 27 ++++++++++ src/main/include/constants/DriveConstants.h | 4 ++ src/main/include/subsystems/DriveSubsystem.h | 11 +++++ src/main/include/subsystems/MAXSwerveModule.h | 10 ++++ 8 files changed, 144 insertions(+), 4 deletions(-) create mode 100644 src/main/include/constants/CharacterizationConstants.h diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 7fb5a697..df6b034e 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -139,7 +139,7 @@ void RobotContainer::ConfigureButtonBindings() { [this] { return m_driverController.GetRightTriggerAxis(); }, [this] { return 0; }) .ToPtr()); - +#ifndef USING_SYSID m_driverController.B().OnTrue( m_leds.Intaking() .AndThen(m_leds.AngryFace()) @@ -171,6 +171,19 @@ void RobotContainer::ConfigureButtonBindings() { &m_scoring, &m_intake, &m_arm)) .AndThen(m_leds.BlinkingFace()) .AndThen(m_leds.Idling())); +#elif USING_SYSID + m_driverController.A().OnTrue( + m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward)); + + m_driverController.B().OnTrue( + m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); + + m_driverController.X().OnTrue( + m_drive.SysIdDynamic(frc2::sysid::Direction::kForward)); + + m_driverController.Y().OnTrue( + m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse)); +#endif m_driverController.LeftBumper() .OnTrue(m_leds.Climbing().AndThen(m_leds.AmogusFace())) diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 4eec9612..24bea9d5 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -61,6 +61,11 @@ DriveSubsystem::DriveSubsystem(PhotonVisionEstimators* vision) }, this); + m_driveModules = {&m_frontLeft, &m_rearLeft, &m_frontRight, &m_rearRight}; + + m_sysIdRoutine = + makeSysIdRoutine(kMotorNames, m_driveModules, MotorType::DriveMotor); + m_publisher = nt::NetworkTableInstance::GetDefault() .GetStructArrayTopic("/SwerveStates") @@ -71,6 +76,33 @@ DriveSubsystem::DriveSubsystem(PhotonVisionEstimators* vision) .Publish(); } +std::unique_ptr DriveSubsystem::makeSysIdRoutine( + std::vector motorNames, std::vector modules, + MotorType motorType) { + return std::make_unique( + frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr}, + frc2::sysid::Mechanism{ + [this, motorType](units::volt_t driveVoltage) { + for (auto* module : m_driveModules) { + module->SetMotorVoltage(motorType, driveVoltage); + } + }, + [this, motorNames, motorType](frc::sysid::SysIdRoutineLog* log) { + ConsoleWriter.logVerbose("LOG WRITE", "log write called%s", ""); + for (size_t i = 0; i < motorNames.size(); i++) { + log->Motor(motorNames[i]) + .voltage( + units::volt_t{m_driveModules[i]->Get(motorType) * + frc::RobotController::GetBatteryVoltage()}) + .position( + units::meter_t{m_driveModules[i]->GetDistance(motorType)}) + .velocity(units::meters_per_second_t{ + m_driveModules[i]->GetRate(motorType)}); + } + }, + this}); +} + void DriveSubsystem::SimulationPeriodic() { logDrivebase(); @@ -104,6 +136,16 @@ void DriveSubsystem::Periodic() { } } +frc2::CommandPtr DriveSubsystem::SysIdQuasistatic( + frc2::sysid::Direction direction) { + return m_sysIdRoutine->Quasistatic(direction); +} + +frc2::CommandPtr DriveSubsystem::SysIdDynamic( + frc2::sysid::Direction direction) { + return m_sysIdRoutine->Dynamic(direction); +} + void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative, @@ -194,10 +236,11 @@ void DriveSubsystem::logDrivebase() { m_rearLeft.GetState(), m_rearRight.GetState()}; std::span states(states_vec.begin(), states_vec.end()); - std::vector desired_vec = {m_frontLeft.GetDesiredState(), m_frontRight.GetDesiredState(), - m_rearLeft.GetDesiredState(), m_rearRight.GetDesiredState()}; + std::vector desired_vec = { + m_frontLeft.GetDesiredState(), m_frontRight.GetDesiredState(), + m_rearLeft.GetDesiredState(), m_rearRight.GetDesiredState()}; std::span desiredStates(desired_vec.begin(), - desired_vec.end()); + desired_vec.end()); m_publisher.Set(states); m_desiredPublisher.Set(desiredStates); diff --git a/src/main/cpp/subsystems/MAXSwerveModule.cpp b/src/main/cpp/subsystems/MAXSwerveModule.cpp index 04bf9d76..b6194049 100644 --- a/src/main/cpp/subsystems/MAXSwerveModule.cpp +++ b/src/main/cpp/subsystems/MAXSwerveModule.cpp @@ -162,6 +162,35 @@ void MAXSwerveModule::SetDesiredState( } } +void MAXSwerveModule::SetMotorVoltage(MotorType type, units::volt_t voltage) { + if (type == MotorType::DriveMotor) { + m_drivingSparkMax.SetVoltage(voltage); + return; + } + m_turningSparkMax.SetVoltage(voltage); +} + +double MAXSwerveModule::Get(MotorType type) { + if (type == MotorType::DriveMotor) { + return m_drivingSparkMax.Get(); + } + return m_turningSparkMax.Get(); +} + +double MAXSwerveModule::GetDistance(MotorType type) { + if (type == MotorType::DriveMotor) { + return m_drivingEncoder.GetPosition(); + } + return m_turningAbsoluteEncoder.GetPosition(); +} + +double MAXSwerveModule::GetRate(MotorType type) { + if (type == MotorType::DriveMotor) { + return m_drivingEncoder.GetVelocity(); + } + return m_turningAbsoluteEncoder.GetVelocity(); +} + void MAXSwerveModule::simUpdateDrivePosition( const frc::SwerveModuleState& desiredState) { m_simDriveEncoderVelocity = desiredState.speed; diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 9ce2f563..6767aa8a 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -4,6 +4,8 @@ #pragma once +// #define USING_SYSID + // Don't define as TEST_SWERVE_BOT if not using the testing swerve robot // #define TEST_SWERVE_BOT @@ -23,6 +25,7 @@ #include "constants/AutoConstants.h" #include "constants/BaseSingleAxisSubsystemConstants.h" #include "constants/CANConstants.h" +#include "constants/CharacterizationConstants.h" #include "constants/ClimbConstants.h" #include "constants/DriveConstants.h" #include "constants/IntakingConstants.h" diff --git a/src/main/include/constants/CharacterizationConstants.h b/src/main/include/constants/CharacterizationConstants.h new file mode 100644 index 00000000..25b0e8ba --- /dev/null +++ b/src/main/include/constants/CharacterizationConstants.h @@ -0,0 +1,27 @@ +#pragma once + +namespace CharacterizationConstants { +namespace FrontLeftDrive { +constexpr double kS = 0.43779; +constexpr double kV = 2.4439; +constexpr double kA = 0.36441; +} // namespace FrontLeftDrive + +namespace FrontRightDrive { +constexpr double kS = 0.47478; +constexpr double kV = 2.4511; +constexpr double kA = 0.36441; +} // namespace FrontRightDrive + +namespace RearLeftDrive { +constexpr double kS = 0.43972; +constexpr double kV = 2.4352; +constexpr double kA = 0.35057; +} // namespace RearLeftDrive + +namespace RearRightDrive { +constexpr double kS = 0.41185; +constexpr double kV = 2.4218; +constexpr double kA = 0.41332; +} // namespace RearRightDrive +} // namespace CharacterizationConstants \ No newline at end of file diff --git a/src/main/include/constants/DriveConstants.h b/src/main/include/constants/DriveConstants.h index 96a874e8..7fd1f15e 100644 --- a/src/main/include/constants/DriveConstants.h +++ b/src/main/include/constants/DriveConstants.h @@ -47,4 +47,8 @@ constexpr int kRearLeftTurningCanId = 5; constexpr units::second_t kLoopTime = 0.022_s; constexpr double kLoopsPerSecond = 1_s / kLoopTime; + +const std::vector kMotorNames = { + "Front-Left-Drive", "Rear-Left-Drive", "Front-Right-Drive", + "Rear-Right-Drive"}; } // namespace DriveConstants \ No newline at end of file diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 1173d200..ba74de09 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -156,6 +157,9 @@ class DriveSubsystem : public frc2::SubsystemBase { frc::Translation2d{-DriveConstants::kWheelBase / 2, -DriveConstants::kTrackWidth / 2}}; + frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction); + frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction); + private: // Components (e.g. motor controllers and sensors) should generally be // declared private and exposed only through public methods. @@ -167,6 +171,13 @@ class DriveSubsystem : public frc2::SubsystemBase { units::radians_per_second_t rot, bool fieldRelative); + std::unique_ptr makeSysIdRoutine( + std::vector names, std::vector modules, + MotorType motorType); + + std::vector m_driveModules; + std::unique_ptr m_sysIdRoutine; + MAXSwerveModule m_frontLeft; MAXSwerveModule m_rearLeft; MAXSwerveModule m_frontRight; diff --git a/src/main/include/subsystems/MAXSwerveModule.h b/src/main/include/subsystems/MAXSwerveModule.h index 555a3440..10ab8f19 100644 --- a/src/main/include/subsystems/MAXSwerveModule.h +++ b/src/main/include/subsystems/MAXSwerveModule.h @@ -15,6 +15,8 @@ #include "Constants.h" +enum class MotorType { DriveMotor = 0, TurnMotor }; + class MAXSwerveModule { public: /** @@ -62,6 +64,14 @@ class MAXSwerveModule { */ void ResetEncoders(); + void SetMotorVoltage(MotorType type, units::volt_t voltage); + + double Get(MotorType type); + + double GetDistance(MotorType type); + + double GetRate(MotorType type); + private: rev::CANSparkMax m_drivingSparkMax; rev::CANSparkMax m_turningSparkMax;