From 554144d0b39ba3870a2e27aea43413d79621fe55 Mon Sep 17 00:00:00 2001
From: Jordan <117786700+jordanjcoderman@users.noreply.github.com>
Date: Wed, 17 Apr 2024 19:44:26 -0700
Subject: [PATCH] Better amp area floor shot

---
 src/main/java/frc/robot/config/CompConfig.java          | 3 +++
 src/main/java/frc/robot/robot_manager/RobotManager.java | 5 ++++-
 src/main/java/frc/robot/vision/VisionSubsystem.java     | 3 ++-
 3 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/config/CompConfig.java b/src/main/java/frc/robot/config/CompConfig.java
index d0f2d01e..390108d8 100644
--- a/src/main/java/frc/robot/config/CompConfig.java
+++ b/src/main/java/frc/robot/config/CompConfig.java
@@ -102,6 +102,9 @@ class CompConfig {
                 floorSpotDistanceToRPM.put(3.0, 1800.0);
                 floorSpotDistanceToRPM.put(5.8, 2700.0);
                 floorSpotDistanceToRPM.put(6.5, 2700.0);
+                floorSpotDistanceToRPM.put(500.0, 2700.0);
+                // Evil hacky way to have alternate shooter RPM for the amp area shot
+                floorSpotDistanceToRPM.put(581.0, 3200.0);
               }),
           new ClimberConfig(
               19,
diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java
index a63ecccf..72d69d05 100644
--- a/src/main/java/frc/robot/robot_manager/RobotManager.java
+++ b/src/main/java/frc/robot/robot_manager/RobotManager.java
@@ -89,7 +89,10 @@ public void robotPeriodic() {
     Rotation2d wristAngleForSpeaker = wrist.getAngleFromDistanceToSpeaker(speakerDistance);
     Rotation2d wristAngleForFloorSpot = wrist.getAngleFromDistanceToFloorSpot(floorSpotDistance);
     shooter.setSpeakerDistance(speakerDistance);
-    shooter.setFloorSpotDistance(floorSpotDistance);
+    shooter.setFloorSpotDistance(
+        floorSpotDistance > VisionSubsystem.FLOOR_SPOT_MAX_DISTANCE_FOR_SUBWOOFER
+            ? 581.0
+            : floorSpotDistance);
 
     // State transitions from requests
     for (RobotFlag flag : flags.getChecked()) {
diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java
index 966a1bfe..d780c5de 100644
--- a/src/main/java/frc/robot/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/vision/VisionSubsystem.java
@@ -25,6 +25,7 @@
 import org.littletonrobotics.junction.Logger;
 
 public class VisionSubsystem extends LifecycleSubsystem {
+  public static final double FLOOR_SPOT_MAX_DISTANCE_FOR_SUBWOOFER = 14.0;
   private static final boolean CALIBRATION_RIG_ENABLED = false;
   private static final boolean SHOOT_TO_SIDE_ENABLED = true;
   public static final boolean LIMELIGHT_UPSIDE_DOWN = true;
@@ -269,7 +270,7 @@ public DistanceAngle getDistanceAngleFloorShot() {
     var usedGoalPose = goalPoseSubwoofer;
     var result = subwooferSpotDistance;
 
-    if (subwooferSpotDistance.distance() > 14.0) {
+    if (subwooferSpotDistance.distance() > FLOOR_SPOT_MAX_DISTANCE_FOR_SUBWOOFER) {
       result = distanceToTargetPose(goalPoseAmpArea, usedRobotPose);
       usedGoalPose = goalPoseAmpArea;
     }