Intake safety

Greater-Rochester-Robotics · Apr 26, 2024 · 8663eb5 · 8663eb5
1 parent cebd221
commit 8663eb5
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 16 deletions.
diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java
@@ -150,16 +150,16 @@ public static final class IntakeConstants {
         // Positions
         public static final double DOWN_POSITION = Math.toRadians(0.0);
         public static final double HANDOFF_POSITION = Math.toRadians(0.0);
-        public static final double JUGGLE_HANDOFF_POSITION = Math.toRadians(95.0);
+        public static final double JUGGLE_HANDOFF_POSITION = Math.toRadians(105.0);
         public static final double SAFE_POSITION = Math.toRadians(30.0);
         public static final double POOP_POSITION = Math.toRadians(85.0);
         public static final double RETRACT_POSITION = Math.toRadians(65.0);
-        public static final double UPRIGHT_POSITION = Math.toRadians(90.0);
+        public static final double UPRIGHT_POSITION = Math.toRadians(120.0);
         public static final double BARF_POSITION = Math.toRadians(14.0);
         public static final double PID_INACTIVE_POSITION = Math.toRadians(1.0);
 
         // Misc
-        public static final double CLOSED_LOOP_ERR = Math.toRadians(5.0);
+        public static final double CLOSED_LOOP_ERR = Math.toRadians(3.0);
         public static final double ALLOWABLE_DIFFERENCE = Math.toRadians(15.0);
 
         // Arm Hardware Configs
@@ -190,7 +190,7 @@ public static final class ArmConfigs {
                 .setZeroOffset(4.843);
 
             public static final SparkPIDControllerConfig PID = new SparkPIDControllerConfig()
-                .setPID(1.1, 0.0007, 0.0)
+                .setPID(1.0, 0.0007, 0.0)
                 .setIZone(Math.toRadians(15.0))
                 .setPositionPIDWrappingEnabled(true)
                 .setPositionPIDWrappingInputLimits(0.0, REL_ENC_FACTOR);

diff --git a/src/main/java/org/team340/robot/RobotContainer.java b/src/main/java/org/team340/robot/RobotContainer.java
@@ -102,13 +102,13 @@ private static void configBindings() {
         driver.y().whileTrue(feeder.shoot());
 
         // Right Joystick Up + Pressed => Intake up
-        driver.rightJoystickUp().and(driver.rightStick()).and(bumpersReleased).onTrue(intake.uprightPosition());
+        driver.rightJoystickUp().and(driver.rightStick()).and(bumpersReleased).onTrue(Routines.uprightPosition());
 
         // Right Joystick Down + Pressed => Intake down
-        driver.rightJoystickDown().and(driver.rightStick()).and(bumpersReleased).onTrue(intake.safePosition());
+        driver.rightJoystickDown().and(driver.rightStick()).and(bumpersReleased).onTrue(Routines.safePosition());
 
         // POV Up => Barf Forward (Hold)
-        driver.povUp().whileTrue(Routines.barfForward()).onFalse(intake.safePosition());
+        driver.povUp().whileTrue(Routines.barfForward()).onFalse(Routines.safePosition());
 
         // POV Down => Pivot Down (Tap)
         driver.povDown().onTrue(pivot.goTo(PivotConstants.DOWN_POSITION));

diff --git a/src/main/java/org/team340/robot/commands/Routines.java b/src/main/java/org/team340/robot/commands/Routines.java
@@ -16,6 +16,28 @@ private Routines() {
         throw new UnsupportedOperationException("This is a utility class!");
     }
 
+    /**
+     * Safely returns the intake to the safe position.
+     */
+    public static Command safePosition() {
+        return parallel(
+            pivot.goTo(PivotConstants.INTAKE_SAFE_POSITION_CLEARED).unless(pivot::isSafeForIntake),
+            sequence(deadline(waitUntil(pivot::isSafeForIntake), intake.maintainPosition()), intake.safePosition())
+        )
+            .withName("Routines.safePosition()");
+    }
+
+    /**
+     * Safely brings the intake to the upright position.
+     */
+    public static Command uprightPosition() {
+        return parallel(
+            pivot.goTo(PivotConstants.INTAKE_SAFE_POSITION_CLEARED).unless(pivot::isSafeForIntake),
+            sequence(deadline(waitUntil(pivot::isSafeForIntake), intake.maintainPosition()), intake.uprightPosition())
+        )
+            .withName("Routines.uprightPosition()");
+    }
+
     /**
      * Deploys and runs the intake. After a note is collected, it is seated by the feeder.
      */
@@ -37,7 +59,7 @@ public static Command intake() {
      * Finishes the intake sequence.
      */
     public static Command finishIntake() {
-        return parallel(feeder.seat(), intake.safePosition()).withName("Routines.finishIntake()");
+        return parallel(feeder.seat(), safePosition()).withName("Routines.finishIntake()");
     }
 
     /**
@@ -176,7 +198,7 @@ public static Command juggle2() {
             repeatingSequence(
                 handoff(),
                 intake.juggleHandoffPosition(),
-                deadline(pivot.goTo(PivotConstants.MAX_POS), intake.maintainPosition()),
+                pivot.goTo(PivotConstants.MAX_POS),
                 deadline(
                     waitUntil(feeder::hasNote).andThen(waitSeconds(0.1)),
                     intake.juggleHandoff(),

diff --git a/src/main/java/org/team340/robot/subsystems/Intake.java b/src/main/java/org/team340/robot/subsystems/Intake.java
@@ -218,15 +218,20 @@ public Command handoff() {
 
     /**
      * Juggles a note into the shooter. Does not end.
+     * Arm is disabled while this command is running.
      */
     public Command juggleHandoff() {
-        return useState(
-            IntakeConstants.JUGGLE_HANDOFF_POSITION,
-            IntakeConstants.JUGGLE_HANDOFF_UPPER_SPEED,
-            IntakeConstants.JUGGLE_HANDOFF_LOWER_SPEED,
-            false
-        )
-            .withName("intake.juggleHandoff()");
+        return commandBuilder("intake.juggleHandoff()")
+            .onInitialize(() -> {
+                rollerUpperMotor.set(IntakeConstants.JUGGLE_HANDOFF_UPPER_SPEED);
+                rollerLowerMotor.set(IntakeConstants.JUGGLE_HANDOFF_LOWER_SPEED);
+                armLeftMotor.stopMotor();
+                armRightMotor.stopMotor();
+            })
+            .onEnd(() -> {
+                rollerUpperMotor.stopMotor();
+                rollerLowerMotor.stopMotor();
+            });
     }
 
     /**