fixed LUTs for shooter

frc2053 · Oct 5, 2024 · 3808204 · 3808204
1 parent 6a45868
commit 3808204
Show file tree

Hide file tree

Showing 6 changed files with 54 additions and 6 deletions.
diff --git 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
@@ -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
@@ -8,7 +8,6 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <frc/DataLogManager.h>
 
-
 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<consts::shooter::PRESET_SPEEDS()> preset) {
-    return frc2::cmd::Run([this, preset] {
+frc2::CommandPtr ShooterSubsystem::RunShooter(std::function<consts::shooter::PRESET_SPEEDS()> 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<units::meter_t, consts::shooter::ShooterSpeeds>& 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);
+    }
 }
diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt
@@ -1 +1 @@
-28d4f57
+6a45868
diff --git 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<units::meter_t, ShooterSpeeds> SHOOTER_LUT;
+struct MeterHash {
+  size_t operator()(const units::meter_t& m) const {
+    return std::hash<double>{}(m.value());
+  }
+};
+
+inline static wpi::interpolating_map<units::meter_t, units::turns_per_second_t> TOP_SHOOTER_LUT{};
+inline static wpi::interpolating_map<units::meter_t, units::turns_per_second_t> BOTTOM_SHOOTER_LUT{};
 
 enum class PRESET_SPEEDS {
     OFF,

diff --git 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<units::meter_t, consts::shooter::ShooterSpeeds>& speeds);
 
-  frc2::CommandPtr RunShooter(std::function<consts::shooter::PRESET_SPEEDS()> preset);
+  frc2::CommandPtr RunShooter(std::function<consts::shooter::PRESET_SPEEDS()> 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);