From 8663eb50c3668a13ee45cbb596c98c472e287bb3 Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Fri, 26 Apr 2024 14:29:10 -0400 Subject: [PATCH] Intake safety --- .../java/org/team340/robot/Constants.java | 8 +++--- .../org/team340/robot/RobotContainer.java | 6 ++--- .../org/team340/robot/commands/Routines.java | 26 +++++++++++++++++-- .../org/team340/robot/subsystems/Intake.java | 19 +++++++++----- 4 files changed, 43 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index 0907859..5aff120 100644 --- 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 index 736af1e..d2ae012 100644 --- 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 index 609d319..f648501 100644 --- 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 index a852f15..daf7864 100644 --- 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(); + }); } /**