Better amp area floor shot

team581 · Apr 18, 2024 · 554144d · 554144d
1 parent 276c5cd
commit 554144d
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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;
     }