fix ground pickup issue

Iron-Panthers · Nov 11, 2023 · 8e417cd · 8e417cd
1 parent 38d200a
commit 8e417cd
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 13 deletions.
diff --git 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
@@ -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);
+  }
 }