From 380820464ccbce8371cd7bcffbb8745c2ade00eb Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Sat, 5 Oct 2024 10:44:26 -0400 Subject: [PATCH] fixed LUTs for shooter --- src/main/cpp/Robot.cpp | 10 ++++++++ src/main/cpp/RobotContainer.cpp | 10 ++++++++ src/main/cpp/subsystems/ShooterSubsystem.cpp | 24 ++++++++++++++++--- src/main/deploy/commit.txt | 2 +- src/main/include/constants/ShooterConstants.h | 11 ++++++++- .../include/subsystems/ShooterSubsystem.h | 3 ++- 6 files changed, 54 insertions(+), 6 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index e1292db..5a93867 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -25,6 +25,16 @@ void Robot::RobotInit() { frc::DriverStation::StartDataLog(frc::DataLogManager::GetLog()); AddPeriodic([this] { m_container.GetSwerveSubsystem().UpdateSwerveOdom(); }, consts::SWERVE_ODOM_LOOP_PERIOD, 2_ms); + + m_container.GetShooterSubsystem().SetupLUTs({ + {1_ft, {1000_rpm, 1000_rpm}}, + {2_ft, {1500_rpm, 1500_rpm}}, + {3_ft, {2000_rpm, 2000_rpm}}, + {4_ft, {2500_rpm, 2500_rpm}}, + {5_ft, {3000_rpm, 3000_rpm}}, + {6_ft, {3500_rpm, 3500_rpm}}, + {7_ft, {4000_rpm, 4000_rpm}}, + }); } void Robot::RobotPeriodic() { diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index dc4ee23..ab14d19 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -74,6 +74,16 @@ void RobotContainer::ConfigureBindings() { controller.A().OnFalse(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::OFF; })); + controller.B().WhileTrue(shooterSubsystem.RunShooter( + [] { return consts::shooter::PRESET_SPEEDS::PASS; })); + controller.B().OnFalse(shooterSubsystem.RunShooter( + [] { return consts::shooter::PRESET_SPEEDS::OFF; })); + + controller.Y().WhileTrue(shooterSubsystem.RunShooter( + [] { return consts::shooter::PRESET_SPEEDS::SUBWOOFER; })); + controller.Y().OnFalse(shooterSubsystem.RunShooter( + [] { return consts::shooter::PRESET_SPEEDS::SUBWOOFER; })); + controller.LeftTrigger().WhileTrue(intakeSubsystem.IntakeNote()); controller.RightTrigger().WhileTrue(intakeSubsystem.PoopNote()); diff --git a/src/main/cpp/subsystems/ShooterSubsystem.cpp b/src/main/cpp/subsystems/ShooterSubsystem.cpp index c2d7468..246ac3b 100644 --- a/src/main/cpp/subsystems/ShooterSubsystem.cpp +++ b/src/main/cpp/subsystems/ShooterSubsystem.cpp @@ -8,7 +8,6 @@ #include #include - ShooterSubsystem::ShooterSubsystem() { ConfigureShooterMotors(consts::shooter::physical::BOTTOM_INVERT, consts::shooter::physical::TOP_INVERT, @@ -20,13 +19,25 @@ ShooterSubsystem::ShooterSubsystem() { frc::SmartDashboard::PutData(this); } -frc2::CommandPtr ShooterSubsystem::RunShooter(std::function preset) { - return frc2::cmd::Run([this, preset] { +frc2::CommandPtr ShooterSubsystem::RunShooter(std::function preset, units::meter_t distance) { + return frc2::cmd::Run([this, preset, distance] { 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::SUBWOOFER: + topWheelVelocitySetpoint = consts::shooter::SUBWOOFER_SPEEDS.topSpeed; + bottomWheelVelocitySetpoint = consts::shooter::SUBWOOFER_SPEEDS.bottomSpeed; + break; + case consts::shooter::PRESET_SPEEDS::PASS: + topWheelVelocitySetpoint = consts::shooter::PASS_SPEEDS.topSpeed; + bottomWheelVelocitySetpoint = consts::shooter::PASS_SPEEDS.bottomSpeed; + break; + case consts::shooter::PRESET_SPEEDS::SPEAKER_DIST: + topWheelVelocitySetpoint = consts::shooter::TOP_SHOOTER_LUT[distance]; + bottomWheelVelocitySetpoint = consts::shooter::BOTTOM_SHOOTER_LUT[distance]; + break; case consts::shooter::PRESET_SPEEDS::OFF: topWheelVelocitySetpoint = 0_rpm; bottomWheelVelocitySetpoint = 0_rpm; @@ -191,4 +202,11 @@ frc2::CommandPtr ShooterSubsystem::BottomWheelSysIdQuasistatic(frc2::sysid::Dire frc2::CommandPtr ShooterSubsystem::BottomWheelSysIdDynamic(frc2::sysid::Direction direction) { return bottomWheelSysIdRoutine.Dynamic(direction).BeforeStarting([this] { runningSysid = true; }).AndThen([this] { runningSysid = false; });; +} + +void ShooterSubsystem::SetupLUTs(const std::map& speeds) { + for(const auto& [key, val] : speeds) { + consts::shooter::TOP_SHOOTER_LUT.insert(key, val.topSpeed); + consts::shooter::BOTTOM_SHOOTER_LUT.insert(key, val.bottomSpeed); + } } \ No newline at end of file diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index a3bc95f..ed8dd96 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -28d4f57 \ No newline at end of file +6a45868 \ No newline at end of file diff --git a/src/main/include/constants/ShooterConstants.h b/src/main/include/constants/ShooterConstants.h index 8af5a9a..5c75dbf 100644 --- a/src/main/include/constants/ShooterConstants.h +++ b/src/main/include/constants/ShooterConstants.h @@ -54,8 +54,17 @@ struct ShooterSpeeds { }; inline constexpr ShooterSpeeds AMP_SPEEDS{3000_rpm, 2000_rpm}; +inline constexpr ShooterSpeeds SUBWOOFER_SPEEDS{5000_rpm, 5000_rpm}; +inline constexpr ShooterSpeeds PASS_SPEEDS{5000_rpm, 5000_rpm}; -static wpi::interpolating_map SHOOTER_LUT; +struct MeterHash { + size_t operator()(const units::meter_t& m) const { + return std::hash{}(m.value()); + } +}; + +inline static wpi::interpolating_map TOP_SHOOTER_LUT{}; +inline static wpi::interpolating_map BOTTOM_SHOOTER_LUT{}; enum class PRESET_SPEEDS { OFF, diff --git a/src/main/include/subsystems/ShooterSubsystem.h b/src/main/include/subsystems/ShooterSubsystem.h index 1529f05..0893df7 100644 --- a/src/main/include/subsystems/ShooterSubsystem.h +++ b/src/main/include/subsystems/ShooterSubsystem.h @@ -27,8 +27,9 @@ class ShooterSubsystem : public frc2::SubsystemBase { */ void Periodic() override; void SimulationPeriodic() override; + void SetupLUTs(const std::map& speeds); - frc2::CommandPtr RunShooter(std::function preset); + frc2::CommandPtr RunShooter(std::function preset, units::meter_t distance=0_m); frc2::CommandPtr TopWheelSysIdQuasistatic(frc2::sysid::Direction direction); frc2::CommandPtr TopWheelSysIdDynamic(frc2::sysid::Direction direction); frc2::CommandPtr BottomWheelSysIdQuasistatic(frc2::sysid::Direction direction);