From 8e417cdf856e4570dd5d0be5d0011a87c686014b Mon Sep 17 00:00:00 2001
From: Jay <jaykang445@gmail.com>
Date: Sat, 11 Nov 2023 09:08:55 -0800
Subject: [PATCH] fix ground pickup issue

---
 src/main/java/frc/robot/RobotContainer.java   | 26 +++++++++----------
 .../commands/GroundIntakeElevatorCommand.java | 15 +++++++++++
 2 files changed, 28 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9de9900..405aa3b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -600,19 +600,19 @@ public void end(boolean interrupted) {
         "N9 1Cone + Mobility",
         new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
 
-    autoSelector.addOption(
-        "Score High Cone [DOES NOT CALIBRATE]",
-        new SetZeroModeCommand(
-                elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check
-            // later
-            .raceWith(
-                new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)
-                    .andThen(
-                        new ScoreCommand(
-                            intakeSubsystem,
-                            elevatorSubsystem,
-                            Constants.SCORE_STEP_MAP.get(
-                                NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))));
+    // autoSelector.addOption(
+    //     "Score High Cone [DOES NOT CALIBRATE]",
+    //     new SetZeroModeCommand(
+    //             elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check
+    //         // later
+    //         .raceWith(
+    //             new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)
+    //                 .andThen(
+    //                     new ScoreCommand(
+    //                         intakeSubsystem,
+    //                         elevatorSubsystem,
+    //                         Constants.SCORE_STEP_MAP.get(
+    //                             NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))));
 
     driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0);
 
diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java
index f84e59f..5261718 100644
--- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java
+++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java
@@ -2,7 +2,10 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants;
+import frc.robot.Constants.Elevator;
 import frc.robot.subsystems.ElevatorSubsystem;
+import frc.util.Util;
+
 import java.util.function.BooleanSupplier;
 
 public class GroundIntakeElevatorCommand extends CommandBase {
@@ -25,4 +28,16 @@ public void execute() {
       elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE);
     }
   }
+
+  @Override
+  public boolean isFinished() {
+    return Util.epsilonEquals(
+              elevatorSubsystem.getCurrentAngleDegrees(),
+              elevatorSubsystem.getTargetAngle(),
+              Elevator.ANGLE_EPSILON)
+          && Util.epsilonEquals(
+              elevatorSubsystem.getCurrentExtensionInches(),
+              elevatorSubsystem.getTargetExtension(),
+              Elevator.EXTENSION_EPSILON);
+  }
 }