diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 3e53921..6c70da0 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -49,6 +49,8 @@ void RobotContainer::ConfigureBindings() { // controller.X().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kForward)); // controller.Y().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kReverse)); + controller.POVDown().OnTrue(swerveSubsystem.TuneSteerPID([this] { return controller.Start().Get(); })); + controller.A().WhileTrue(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::AMP; })); controller.A().OnFalse(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::OFF; })); diff --git a/src/main/cpp/str/SwerveDrive.cpp b/src/main/cpp/str/SwerveDrive.cpp index a78ecf9..a735266 100644 --- a/src/main/cpp/str/SwerveDrive.cpp +++ b/src/main/cpp/str/SwerveDrive.cpp @@ -182,7 +182,6 @@ void SwerveDrive::UpdateNTEntries() { lookaheadPub.Set(GetPredictedPose(1_s, 1_s)); estimatorPub.Set(GetPose()); isSlippingPub.Set(IsSlipping()); - steerGainsEntry.Set(steerGainsMk4i); } void SwerveDrive::SimulationPeriodic() { @@ -337,3 +336,13 @@ bool SwerveDrive::IsSlipping() { }); return (maxIt->speed / minIt->speed) > slipCoeff; } + +str::SwerveModuleSteerGains SwerveDrive::GetSteerGains() const { + return modules[0].GetSteerGains(); +} + +void SwerveDrive::SetSteerGains(str::SwerveModuleSteerGains newGains) { + for(int i = 0; i < 4; i++) { + modules[i].SetSteerGains(newGains); + } +} \ No newline at end of file diff --git a/src/main/cpp/str/SwerveModule.cpp b/src/main/cpp/str/SwerveModule.cpp index 8b79e48..0872ffc 100644 --- a/src/main/cpp/str/SwerveModule.cpp +++ b/src/main/cpp/str/SwerveModule.cpp @@ -328,6 +328,10 @@ void SwerveModule::SetSteerGains(str::SwerveModuleSteerGains newGains) { } } +str::SwerveModuleSteerGains SwerveModule::GetSteerGains() const { + return steerGains; +} + void SwerveModule::SetDriveGains(str::SwerveModuleDriveGains newGains) { driveGains = newGains; ctre::phoenix6::configs::Slot0Configs driveSlotConfig{}; diff --git a/src/main/cpp/str/struct/SwerveHelperStruct.cpp b/src/main/cpp/str/struct/SwerveHelperStruct.cpp deleted file mode 100644 index 77aa54a..0000000 --- a/src/main/cpp/str/struct/SwerveHelperStruct.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include "str/struct/SwerveHelperStruct.h" - -namespace { -constexpr size_t start = 0; -constexpr size_t one = start + 8; -constexpr size_t two = one + 8; -constexpr size_t three = two + 8; -constexpr size_t four = three + 8; -constexpr size_t five = four + 8; -constexpr size_t six = five + 8; -constexpr size_t seven = six + 8; -constexpr size_t eight = seven + 8; -} // namespace - -using SteerStructType = wpi::Struct; - -str::SwerveModuleSteerGains SteerStructType::Unpack(std::span data) { - return str::SwerveModuleSteerGains{ - units::turns_per_second_t{wpi::UnpackStruct(data)}, - str::gains::radial::turn_volt_ka_unit_t{wpi::UnpackStruct(data)}, - str::gains::radial::turn_volt_kv_unit_t{wpi::UnpackStruct(data)}, - str::gains::radial::turn_amp_ka_unit_t{wpi::UnpackStruct(data)}, - str::gains::radial::turn_amp_kv_unit_t{wpi::UnpackStruct(data)}, - units::ampere_t{wpi::UnpackStruct(data)}, - str::gains::radial::turn_amp_kp_unit_t{wpi::UnpackStruct(data)}, - str::gains::radial::turn_amp_ki_unit_t{wpi::UnpackStruct(data)}, - str::gains::radial::turn_amp_kd_unit_t{wpi::UnpackStruct(data)}, - }; -} - -void SteerStructType::Pack(std::span data, - const str::SwerveModuleSteerGains& value) { - wpi::PackStruct(data, value.motionMagicCruiseVel.value()); - wpi::PackStruct(data, value.motionMagicExpoKa.value()); - wpi::PackStruct(data, value.motionMagicExpoKv.value()); - wpi::PackStruct(data, value.kA.value()); - wpi::PackStruct(data, value.kV.value()); - wpi::PackStruct(data, value.kS.value()); - wpi::PackStruct(data, value.kP.value()); - wpi::PackStruct(data, value.kI.value()); - wpi::PackStruct(data, value.kD.value()); -} \ No newline at end of file diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp index ec53bbc..5d5d6d2 100644 --- a/src/main/cpp/subsystems/SwerveSubsystem.cpp +++ b/src/main/cpp/subsystems/SwerveSubsystem.cpp @@ -438,9 +438,58 @@ frc2::CommandPtr SwerveSubsystem::WheelRadius(frc2::sysid::Direction dir) { } frc2::CommandPtr SwerveSubsystem::TuneSteerPID(std::function isDone) { + std::string tablePrefix = "SwerveDrive/steerGains/"; return frc2::cmd::Sequence( - frc2::cmd::RunOnce([this] { - - }) - ); + frc2::cmd::RunOnce( + [tablePrefix, this] { + frc::SmartDashboard::PutNumber(tablePrefix + "setpoint", 0); + frc::SmartDashboard::PutNumber( + tablePrefix + "mmCruiseVel", consts::swerve::gains::MK4I_STEER_CRUISE_VEL.value()); + frc::SmartDashboard::PutNumber( + tablePrefix + "mmKA", consts::swerve::gains::MK4I_STEER_MOTION_MAGIC_KA.value()); + frc::SmartDashboard::PutNumber( + tablePrefix + "mmKV", consts::swerve::gains::MK4I_STEER_MOTION_MAGIC_KV.value()); + frc::SmartDashboard::PutNumber( + tablePrefix + "kA", consts::swerve::gains::MK4I_STEER_KA.value()); + frc::SmartDashboard::PutNumber( + tablePrefix + "kV", consts::swerve::gains::MK4I_STEER_KV.value()); + frc::SmartDashboard::PutNumber( + tablePrefix + "kS", consts::swerve::gains::MK4I_STEER_KS.value()); + frc::SmartDashboard::PutNumber( + tablePrefix + "kP", consts::swerve::gains::MK4I_STEER_KP.value()); + frc::SmartDashboard::PutNumber( + tablePrefix + "kI", consts::swerve::gains::MK4I_STEER_KI.value()); + frc::SmartDashboard::PutNumber( + tablePrefix + "kD", consts::swerve::gains::MK4I_STEER_KD.value()); + frc::SwerveModuleState zeroState{0_mps, frc::Rotation2d{0_rad}}; + swerveDrive.SetModuleStates({zeroState,zeroState,zeroState,zeroState}); + }, + {this}), + frc2::cmd::Run( + [this, tablePrefix] { + str::SwerveModuleSteerGains newGains{ + units::turns_per_second_t{frc::SmartDashboard::GetNumber(tablePrefix + "mmCruiseVel", 0)}, + str::gains::radial::turn_volt_ka_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "mmKA", 0)}, + str::gains::radial::turn_volt_kv_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "mmKV", 0)}, + str::gains::radial::turn_amp_ka_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kA", 0)}, + str::gains::radial::turn_amp_kv_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kV", 0)}, + units::ampere_t{frc::SmartDashboard::GetNumber(tablePrefix + "kS", 0)}, + str::gains::radial::turn_amp_kp_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kP", 0)}, + str::gains::radial::turn_amp_ki_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kI", 0)}, + str::gains::radial::turn_amp_kd_unit_t{frc::SmartDashboard::GetNumber(tablePrefix + "kD", 0)} + }; + + if (newGains != swerveDrive.GetSteerGains()) { + for (int i = 0; i < 4; i++) { + swerveDrive.SetSteerGains(newGains); + } + } + + for (int i = 0; i < 4; i++) { + frc::SwerveModuleState state{0_mps, frc::Rotation2d{units::degree_t{frc::SmartDashboard::GetNumber(tablePrefix + "setpoint", 0)}}}; + swerveDrive.SetModuleStates({state, state, state, state}); + } + }, + {this}) + .Until(isDone)); } \ No newline at end of file diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index 40cad79..dd663a8 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -fdc9431 \ No newline at end of file +44febdd \ No newline at end of file diff --git a/src/main/include/str/SwerveDrive.h b/src/main/include/str/SwerveDrive.h index 16336f3..600ec4f 100644 --- a/src/main/include/str/SwerveDrive.h +++ b/src/main/include/str/SwerveDrive.h @@ -11,7 +11,6 @@ #include #include #include -#include "str/struct/SwerveHelperStruct.h" #include #include @@ -63,6 +62,9 @@ class SwerveDrive { void LogMk4nSteerVoltage(frc::sysid::SysIdRoutineLog *log); void LogDriveVoltage(frc::sysid::SysIdRoutineLog *log); + str::SwerveModuleSteerGains GetSteerGains() const; + void SetSteerGains(str::SwerveModuleSteerGains newGains); + private: std::array ConvertModuleForcesToTorqueCurrent( const std::array &xForce, @@ -176,7 +178,5 @@ class SwerveDrive { nt->GetStructTopic("PoseEstimatorPose").Publish()}; nt::StructPublisher lookaheadPub{ nt->GetStructTopic("LookaheadPose").Publish()}; - nt::StructEntry steerGainsEntry{ - nt->GetStructTopic("SteerGains").GetEntry(steerGainsMk4i)}; }; } // namespace str diff --git a/src/main/include/str/SwerveModule.h b/src/main/include/str/SwerveModule.h index a9a83d7..60b22e7 100644 --- a/src/main/include/str/SwerveModule.h +++ b/src/main/include/str/SwerveModule.h @@ -47,6 +47,7 @@ class SwerveModule { void SetDriveToVoltage(units::volt_t voltsToSend); void SetSteerGains(str::SwerveModuleSteerGains newGains); void SetDriveGains(str::SwerveModuleDriveGains newGains); + str::SwerveModuleSteerGains GetSteerGains() const; private: bool ConfigureSteerMotor(bool invertSteer, units::scalar_t steerGearing, diff --git a/src/main/include/str/struct/SwerveHelperStruct.h b/src/main/include/str/struct/SwerveHelperStruct.h deleted file mode 100644 index 0a12263..0000000 --- a/src/main/include/str/struct/SwerveHelperStruct.h +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include - -#include "str/SwerveModuleHelpers.h" - -template <> -struct wpi::Struct { - static constexpr std::string_view GetTypeName() { return "SwerveModuleSteerGains"; } - static constexpr size_t GetSize() { return 72; } - static constexpr std::string_view GetSchema() { return "double motionMagicCruiseVel;double motionMagicExpoKa;double motionMagicExpoKv;double kA;double kV;double kS;double kP;double kI;double kD;"; } - - static str::SwerveModuleSteerGains Unpack(std::span data); - static void Pack(std::span data, const str::SwerveModuleSteerGains& value); -}; - -static_assert(wpi::StructSerializable); - -template <> -struct wpi::Struct { - static constexpr std::string_view GetTypeName() { return "SwerveModuleDriveGains"; } - static constexpr size_t GetSize() { return 48; } - static constexpr std::string_view GetSchema() { return "double kA;double kV;double kS;double kP;double kI;double kD;"; } - - static str::SwerveModuleDriveGains Unpack(std::span data); - static void Pack(std::span data, const str::SwerveModuleDriveGains& value); -}; - -static_assert(wpi::StructSerializable); \ No newline at end of file