From f5a5c55c1bb847f458d8f5af6718cd578179c61d Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Fri, 30 Aug 2024 14:25:53 -0400 Subject: [PATCH] working shooter control --- src/main/cpp/RobotContainer.cpp | 29 +++-- src/main/cpp/subsystems/ShooterSubsystem.cpp | 100 ++++++++++++++---- src/main/deploy/commit.txt | 2 +- src/main/include/constants/ShooterConstants.h | 35 ++++-- src/main/include/str/Gains.h | 14 +++ .../include/subsystems/ShooterSubsystem.h | 94 ++++++++++++++-- 6 files changed, 229 insertions(+), 45 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 37faeb4..5139582 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -49,14 +49,27 @@ void RobotContainer::ConfigureBindings() { // controller.X().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kForward)); // controller.Y().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kReverse)); - controller.A().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque( - frc2::sysid::Direction::kForward)); - controller.B().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque( - frc2::sysid::Direction::kReverse)); - controller.X().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque( - frc2::sysid::Direction::kForward)); - controller.Y().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque( - frc2::sysid::Direction::kReverse)); + controller.A().WhileTrue(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::AMP; })); + controller.A().OnFalse(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::OFF; })); + + // controller.A().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kForward)); + // controller.B().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kReverse)); + // controller.X().WhileTrue(shooterSubsystem.TopWheelSysIdDynamic(frc2::sysid::Direction::kForward)); + // controller.Y().WhileTrue(shooterSubsystem.TopWheelSysIdDynamic(frc2::sysid::Direction::kReverse)); + + // (controller.A() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdQuasistatic(frc2::sysid::Direction::kForward)); + // (controller.B() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdQuasistatic(frc2::sysid::Direction::kReverse)); + // (controller.X() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdDynamic(frc2::sysid::Direction::kForward)); + // (controller.Y() && controller.Back()).WhileTrue(shooterSubsystem.BottomWheelSysIdDynamic(frc2::sysid::Direction::kReverse)); + + // controller.A().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque( + // frc2::sysid::Direction::kForward)); + // controller.B().WhileTrue(swerveSubsystem.SysIdSteerMk4nQuasistaticTorque( + // frc2::sysid::Direction::kReverse)); + // controller.X().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque( + // frc2::sysid::Direction::kForward)); + // controller.Y().WhileTrue(swerveSubsystem.SysIdSteerMk4nDynamicTorque( + // frc2::sysid::Direction::kReverse)); // controller.A().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kForward)); // controller.B().WhileTrue(swerveSubsystem.SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction::kReverse)); diff --git a/src/main/cpp/subsystems/ShooterSubsystem.cpp b/src/main/cpp/subsystems/ShooterSubsystem.cpp index a631f44..877a1d1 100644 --- a/src/main/cpp/subsystems/ShooterSubsystem.cpp +++ b/src/main/cpp/subsystems/ShooterSubsystem.cpp @@ -4,6 +4,7 @@ #include "subsystems/ShooterSubsystem.h" #include "constants/Constants.h" +#include ShooterSubsystem::ShooterSubsystem() { ConfigureShooterMotors(consts::shooter::physical::BOTTOM_INVERT, @@ -14,22 +15,68 @@ ShooterSubsystem::ShooterSubsystem() { ConfigureMotorSignals(); } +frc2::CommandPtr ShooterSubsystem::RunShooter(std::function preset) { + return frc2::cmd::Run([this, preset] { + switch(preset()) { + case consts::shooter::PRESET_SPEEDS::AMP: + topWheelVelocitySetpoint = consts::shooter::AMP_SPEEDS.topSpeed; + bottomWheelVelocitySetpoint = consts::shooter::AMP_SPEEDS.bottomSpeed; + break; + case consts::shooter::PRESET_SPEEDS::OFF: + topWheelVelocitySetpoint = 0_rpm; + bottomWheelVelocitySetpoint = 0_rpm; + break; + default: + topWheelVelocitySetpoint = 0_rpm; + bottomWheelVelocitySetpoint = 0_rpm; + break; + } + }, {this}).Until([this] { return IsUpToSpeed(); }); +} + // This method will be called once per scheduler run void ShooterSubsystem::Periodic() { - ctre::phoenix::StatusCode shooterWaitResult = ctre::phoenix6::BaseStatusSignal::RefreshAll({&topMotorVelSig, &topMotorVoltageSig, &bottomMotorVelSig, &bottomMotorVoltageSig}); + ctre::phoenix::StatusCode shooterWaitResult = ctre::phoenix6::BaseStatusSignal::RefreshAll({ + &topMotorPosSig, + &topMotorVelSig, + &topMotorVoltageSig, + &bottomMotorPosSig, + &bottomMotorVelSig, + &bottomMotorVoltageSig + }); if(!shooterWaitResult.IsOK()) { fmt::print("Error grabbing shooter signals! Details: {}\n", shooterWaitResult.GetName()); } + currentTopWheelPosition = ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(topMotorPosSig, topMotorVelSig); + currentBottomWheelPosition = ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(bottomMotorPosSig, bottomMotorVelSig); + currentBottomWheelVelocity = bottomMotorVelSig.GetValue(); currentTopWheelVelocity = topMotorVelSig.GetValue(); - topWheelMotor.SetControl(topMotorSetter.WithOutput(10_V)); - bottomWheelMotor.SetControl(bottomMotorSetter.WithOutput(10_V)); + units::volt_t topFFVolts = topWheelFF.Calculate(topWheelVelocitySetpoint); + units::volt_t bottomFFVolts = bottomWheelFF.Calculate(bottomWheelVelocitySetpoint); + + units::volt_t topPIDOutput = units::volt_t{topWheelPID.Calculate(currentTopWheelVelocity.value(), topWheelVelocitySetpoint.value())}; + units::volt_t bottomPIDOutput = units::volt_t{bottomWheelPID.Calculate(currentTopWheelVelocity.value(), topWheelVelocitySetpoint.value())}; + + if(!runningSysid) { + if(topWheelVelocitySetpoint == 0_rpm) { + topWheelMotor.SetControl(coastSetter); + } + else { + topWheelMotor.SetControl(topMotorVoltageSetter.WithOutput(topFFVolts + topPIDOutput)); + } + if(bottomWheelVelocitySetpoint == 0_rpm) { + bottomWheelMotor.SetControl(coastSetter); + } + else { + bottomWheelMotor.SetControl(bottomMotorVoltageSetter.WithOutput(bottomFFVolts + bottomPIDOutput)); + } + } - bottomWheelVelocityPub.Set(currentBottomWheelVelocity.convert().value()); - topWheelVelocityPub.Set(currentTopWheelVelocity.convert().value()); + UpdateNTEntries(); } void ShooterSubsystem::SimulationPeriodic() { @@ -46,9 +93,14 @@ void ShooterSubsystem::SimulationPeriodic() { bottomMotorSim.SetRotorVelocity(bottomFlywheelSim.GetAngularVelocity()); } -bool ShooterSubsystem::IsUpToSpeed() { - return (units::math::abs(topWheelVelocitySetpoint - currentTopWheelVelocity) < consts::shooter::gains::VEL_TOLERANCE) && - (units::math::abs(bottomWheelVelocitySetpoint - currentBottomWheelVelocity) < consts::shooter::gains::VEL_TOLERANCE); +void ShooterSubsystem::UpdateNTEntries() { + topWheelSetpointPub.Set(topWheelVelocitySetpoint.convert().value()); + bottomWheelSetpointPub.Set(bottomWheelVelocitySetpoint.convert().value()); + + bottomWheelVelocityPub.Set(currentBottomWheelVelocity.convert().value()); + topWheelVelocityPub.Set(currentTopWheelVelocity.convert().value()); + + isUpToSpeedPub.Set(UpToSpeed().Get()); } bool ShooterSubsystem::ConfigureShooterMotors( @@ -56,15 +108,6 @@ bool ShooterSubsystem::ConfigureShooterMotors( units::ampere_t supplyCurrentLimit, units::ampere_t statorCurrentLimit) { ctre::phoenix6::configs::TalonFXConfiguration shooterConfig{}; - ctre::phoenix6::configs::Slot0Configs shooterSlotConfig{}; - - shooterSlotConfig.kV = consts::shooter::gains::SHOOTER_KV.value(); - shooterSlotConfig.kA = consts::shooter::gains::SHOOTER_KA.value(); - shooterSlotConfig.kS = consts::shooter::gains::SHOOTER_KS.value(); - shooterSlotConfig.kP = consts::shooter::gains::SHOOTER_KP.value(); - shooterSlotConfig.kI = consts::shooter::gains::SHOOTER_KI.value(); - shooterSlotConfig.kD = consts::shooter::gains::SHOOTER_KD.value(); - shooterConfig.Slot0 = shooterSlotConfig; shooterConfig.MotorOutput.NeutralMode = ctre::phoenix6::signals::NeutralModeValue::Coast; @@ -100,14 +143,17 @@ bool ShooterSubsystem::ConfigureShooterMotors( } bool ShooterSubsystem::ConfigureMotorSignals() { - topMotorSetter.UpdateFreqHz = 0_Hz; - bottomMotorSetter.UpdateFreqHz = 0_Hz; + topMotorVoltageSetter.UpdateFreqHz = 0_Hz; + bottomMotorVoltageSetter.UpdateFreqHz = 0_Hz; + coastSetter.UpdateFreqHz = 0_Hz; //Double the rio update rate? Not sure what is optimal here units::hertz_t updateRate = 1.0 / (consts::LOOP_PERIOD * 2.0); + topMotorPosSig.SetUpdateFrequency(updateRate); topMotorVelSig.SetUpdateFrequency(updateRate); topMotorVoltageSig.SetUpdateFrequency(updateRate); + bottomMotorPosSig.SetUpdateFrequency(updateRate); bottomMotorVelSig.SetUpdateFrequency(updateRate); bottomMotorVoltageSig.SetUpdateFrequency(updateRate); @@ -124,4 +170,20 @@ bool ShooterSubsystem::ConfigureMotorSignals() { } return optimizeTopMotor.IsOK() && optimizeBottomMotor.IsOK(); +} + +frc2::CommandPtr ShooterSubsystem::TopWheelSysIdQuasistatic(frc2::sysid::Direction direction) { + return topWheelSysIdRoutine.Quasistatic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; }); +} + +frc2::CommandPtr ShooterSubsystem::TopWheelSysIdDynamic(frc2::sysid::Direction direction) { + return topWheelSysIdRoutine.Dynamic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; });; +} + +frc2::CommandPtr ShooterSubsystem::BottomWheelSysIdQuasistatic(frc2::sysid::Direction direction) { + return bottomWheelSysIdRoutine.Quasistatic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; }); +} + +frc2::CommandPtr ShooterSubsystem::BottomWheelSysIdDynamic(frc2::sysid::Direction direction) { + return bottomWheelSysIdRoutine.Dynamic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; });; } \ No newline at end of file diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index 20f4c1a..b4aec7f 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -e8855b1 \ No newline at end of file +d7b9781 \ No newline at end of file diff --git a/src/main/include/constants/ShooterConstants.h b/src/main/include/constants/ShooterConstants.h index 130c2b5..d761626 100644 --- a/src/main/include/constants/ShooterConstants.h +++ b/src/main/include/constants/ShooterConstants.h @@ -7,6 +7,7 @@ #include #include #include +#include #include "str/Gains.h" @@ -34,18 +35,34 @@ inline constexpr frc::DCMotor SHOOTER_MOTOR = frc::DCMotor::Falcon500(1); inline constexpr units::meter_t WHEEL_RADIUS = 2_in; //From onshape doc -static constexpr units::kilogram_square_meter_t FLYWHEEL_MOI = 62.564007 * 1_in * 1_in * 1_lb; +inline constexpr units::kilogram_square_meter_t FLYWHEEL_MOI = 5.01 * 1_in * 1_in * 1_lb; } // namespace physical namespace gains { -inline constexpr units::radians_per_second_t VEL_TOLERANCE = 10_rpm; -inline constexpr str::gains::radial::turn_volt_ka_unit_t SHOOTER_KA{0}; -inline constexpr str::gains::radial::turn_amp_kv_unit_t SHOOTER_KV{0}; -inline constexpr units::ampere_t SHOOTER_KS{0}; -inline constexpr units::volt_t SHOOTER_KS_V{0}; -inline constexpr str::gains::radial::turn_amp_kp_unit_t SHOOTER_KP{0}; -inline constexpr str::gains::radial::turn_amp_ki_unit_t SHOOTER_KI{0}; -inline constexpr str::gains::radial::turn_amp_kd_unit_t SHOOTER_KD{0}; +inline constexpr units::turns_per_second_t VEL_TOLERANCE = 2_rpm; +inline constexpr str::gains::radial::turn_volt_ka_unit_t SHOOTER_KA{0.021356}; +inline constexpr str::gains::radial::turn_volt_kv_unit_t SHOOTER_KV{0.11227}; +inline constexpr units::volt_t SHOOTER_KS{0.02364}; +inline constexpr str::gains::radial::turn_volt_kp_unit_t SHOOTER_KP{0.047275}; +inline constexpr str::gains::radial::turn_volt_ki_unit_t SHOOTER_KI{0}; +inline constexpr str::gains::radial::turn_volt_kd_unit_t SHOOTER_KD{0}; } // namespace gains + +struct ShooterSpeeds { + units::turns_per_second_t topSpeed; + units::turns_per_second_t bottomSpeed; +}; + +inline constexpr ShooterSpeeds AMP_SPEEDS{3000_rpm, 2000_rpm}; + +static wpi::interpolating_map SHOOTER_LUT; + +enum class PRESET_SPEEDS { + OFF, + AMP, + SPEAKER_DIST, + SUBWOOFER, + PASS +}; } // namespace shooter } // namespace consts diff --git a/src/main/include/str/Gains.h b/src/main/include/str/Gains.h index 91f84e8..3dc2745 100644 --- a/src/main/include/str/Gains.h +++ b/src/main/include/str/Gains.h @@ -27,6 +27,20 @@ using turn_volt_kv_unit = units::compound_unit>; using turn_volt_kv_unit_t = units::unit_t; +using turn_volt_kp_unit = + units::compound_unit>; +using turn_volt_kp_unit_t = units::unit_t; + +using turn_volt_ki_unit = units::compound_unit< + units::volts, + units::inverse>>; +using turn_volt_ki_unit_t = units::unit_t; + +using turn_volt_kd_unit = + units::compound_unit>; +using turn_volt_kd_unit_t = units::unit_t; + using turn_amp_ka_unit = units::compound_unit>; using turn_amp_ka_unit_t = units::unit_t; diff --git a/src/main/include/subsystems/ShooterSubsystem.h b/src/main/include/subsystems/ShooterSubsystem.h index ffa9349..095bff4 100644 --- a/src/main/include/subsystems/ShooterSubsystem.h +++ b/src/main/include/subsystems/ShooterSubsystem.h @@ -11,8 +11,12 @@ #include #include #include - +#include +#include +#include +#include #include "constants/ShooterConstants.h" +#include class ShooterSubsystem : public frc2::SubsystemBase { public: @@ -23,9 +27,26 @@ class ShooterSubsystem : public frc2::SubsystemBase { */ void Periodic() override; void SimulationPeriodic() override; - bool IsUpToSpeed(); + + frc2::CommandPtr RunShooter(std::function preset); + frc2::CommandPtr TopWheelSysIdQuasistatic(frc2::sysid::Direction direction); + frc2::CommandPtr TopWheelSysIdDynamic(frc2::sysid::Direction direction); + frc2::CommandPtr BottomWheelSysIdQuasistatic(frc2::sysid::Direction direction); + frc2::CommandPtr BottomWheelSysIdDynamic(frc2::sysid::Direction direction); + + const frc2::Trigger& UpToSpeed() const { return upToSpeedTrigger; }; private: + + void UpdateNTEntries(); + + bool IsUpToSpeed() { + return (units::math::abs(topWheelVelocitySetpoint - currentTopWheelVelocity) < consts::shooter::gains::VEL_TOLERANCE) && + (units::math::abs(bottomWheelVelocitySetpoint - currentBottomWheelVelocity) < consts::shooter::gains::VEL_TOLERANCE); + }; + frc2::Trigger upToSpeedTrigger{[this] { return IsUpToSpeed(); }}; + + bool ConfigureShooterMotors(bool invertBottom, bool invertTop, units::scalar_t shooterGearing, units::ampere_t supplyCurrentLimit, @@ -35,13 +56,18 @@ class ShooterSubsystem : public frc2::SubsystemBase { ctre::phoenix6::hardware::TalonFX topWheelMotor{consts::shooter::can_ids::TOP_SHOOTER, "*"}; ctre::phoenix6::hardware::TalonFX bottomWheelMotor{consts::shooter::can_ids::BOTTOM_SHOOTER, "*"}; + ctre::phoenix6::StatusSignal topMotorPosSig = topWheelMotor.GetPosition(); ctre::phoenix6::StatusSignal topMotorVelSig = topWheelMotor.GetVelocity(); ctre::phoenix6::StatusSignal topMotorVoltageSig = topWheelMotor.GetMotorVoltage(); - ctre::phoenix6::controls::VoltageOut topMotorSetter{0_V}; + ctre::phoenix6::controls::VoltageOut topMotorVoltageSetter{0_V}; + ctre::phoenix6::StatusSignal bottomMotorPosSig = bottomWheelMotor.GetPosition(); ctre::phoenix6::StatusSignal bottomMotorVelSig = bottomWheelMotor.GetVelocity(); ctre::phoenix6::StatusSignal bottomMotorVoltageSig = bottomWheelMotor.GetMotorVoltage(); - ctre::phoenix6::controls::VoltageOut bottomMotorSetter{0_V}; + ctre::phoenix6::controls::VoltageOut bottomMotorVoltageSetter{0_V}; + + ctre::phoenix6::controls::CoastOut coastSetter{}; + ctre::phoenix6::sim::TalonFXSimState &topMotorSim = topWheelMotor.GetSimState(); ctre::phoenix6::sim::TalonFXSimState &bottomMotorSim = bottomWheelMotor.GetSimState(); @@ -52,6 +78,52 @@ class ShooterSubsystem : public frc2::SubsystemBase { frc::sim::FlywheelSim topFlywheelSim{shooterPlant, consts::shooter::physical::SHOOTER_MOTOR}; frc::sim::FlywheelSim bottomFlywheelSim{shooterPlant, consts::shooter::physical::SHOOTER_MOTOR}; + frc::SimpleMotorFeedforward topWheelFF{ + consts::shooter::gains::SHOOTER_KS, consts::shooter::gains::SHOOTER_KV, consts::shooter::gains::SHOOTER_KA}; + + frc::SimpleMotorFeedforward bottomWheelFF{ + consts::shooter::gains::SHOOTER_KS, consts::shooter::gains::SHOOTER_KV, consts::shooter::gains::SHOOTER_KA}; + + frc::PIDController topWheelPID{ + consts::shooter::gains::SHOOTER_KP.value(), consts::shooter::gains::SHOOTER_KI.value(), consts::shooter::gains::SHOOTER_KD.value() + }; + + frc::PIDController bottomWheelPID{ + consts::shooter::gains::SHOOTER_KP.value(), consts::shooter::gains::SHOOTER_KI.value(), consts::shooter::gains::SHOOTER_KD.value() + }; + + frc2::sysid::SysIdRoutine topWheelSysIdRoutine{ + frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr}, + frc2::sysid::Mechanism{ + [this](units::volt_t driveVoltage) { + topWheelMotor.SetControl(topMotorVoltageSetter.WithOutput(driveVoltage)); + }, + [this](frc::sysid::SysIdRoutineLog* log) { + log->Motor("top-shooter-wheel") + .voltage(topMotorVoltageSig.GetValue()) + .position(currentTopWheelPosition) + .velocity(currentTopWheelVelocity); + }, + this + } + }; + + frc2::sysid::SysIdRoutine bottomWheelSysIdRoutine{ + frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr}, + frc2::sysid::Mechanism{ + [this](units::volt_t driveVoltage) { + bottomWheelMotor.SetControl(bottomMotorVoltageSetter.WithOutput(driveVoltage)); + }, + [this](frc::sysid::SysIdRoutineLog* log) { + log->Motor("bottom-shooter-wheel") + .voltage(bottomMotorVoltageSig.GetValue()) + .position(currentBottomWheelPosition) + .velocity(currentBottomWheelVelocity); + }, + this + } + }; + std::shared_ptr nt{ nt::NetworkTableInstance::GetDefault().GetTable("Shooter")}; nt::DoublePublisher loopTimePub{ @@ -71,9 +143,15 @@ class ShooterSubsystem : public frc2::SubsystemBase { nt::DoublePublisher bottomWheelSetpointPub{ nt->GetDoubleTopic("BottomWheelSetpointRPM").Publish() }; + nt::BooleanPublisher isUpToSpeedPub{ + nt->GetBooleanTopic("IsUpToSpeed").Publish() + }; - units::radians_per_second_t topWheelVelocitySetpoint{0_rpm}; - units::radians_per_second_t bottomWheelVelocitySetpoint{0_rpm}; - units::radians_per_second_t currentTopWheelVelocity{0_rpm}; - units::radians_per_second_t currentBottomWheelVelocity{0_rpm}; + units::turns_per_second_t topWheelVelocitySetpoint{0_rpm}; + units::turns_per_second_t bottomWheelVelocitySetpoint{0_rpm}; + units::turn_t currentTopWheelPosition{0_rad}; + units::turn_t currentBottomWheelPosition{0_rad}; + units::turns_per_second_t currentTopWheelVelocity{0_rpm}; + units::turns_per_second_t currentBottomWheelVelocity{0_rpm}; + bool runningSysid{false}; };