diff --git a/.vscode/settings.json b/.vscode/settings.json index 168fa10..2236597 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.format.settings.url": "eclipse-formatter.xml", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", + "wpilib.autoStartRioLog": false } diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index 68b6fbc..869e8e0 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -12,6 +12,8 @@ public class LazyCan { private LaserCan laserCan; private int canId; + private RegionOfInterest regionOfInterest; + private Measurement measurement; private double threshold; @@ -24,6 +26,8 @@ public class LazyCan { public LazyCan(int canId) { laserCan = new LaserCan(canId); this.canId = canId; + laserCan = new LaserCan(canId); + this.canId = canId; } /** @@ -33,17 +37,20 @@ public LazyCan(int canId) { */ public int getDistanceMm() { measurement = laserCan.getMeasurement(); + measurement = laserCan.getMeasurement(); return measurement != null ? measurement.distance_mm : -1; } /** - * Checks if the LaserCan finds an object within the distance threshold + * Returns true if the LaserCan finds an object within the distance threshold * - * @return True if there is an object within the distance threshold, false otherwise + * @return if there is an object within the distance threshold */ public boolean withinThreshold() { - return getDistanceMm() <= threshold; + measurement = laserCan.getMeasurement(); + + return measurement != null ? measurement.distance_mm <= threshold : false; } /** @@ -53,25 +60,11 @@ public boolean withinThreshold() { * @param y the y start position for the reigon * @param w the width of the reigon * @param h the height of the reigon - * @return The current {@link LazyCan} instance + * @return the current LazyCan Object */ public LazyCan withRegionOfInterest(int x, int y, int w, int h) { - try { - laserCan.setRegionOfInterest(new RegionOfInterest(x, y, w, h)); - } catch (ConfigurationFailedException e) { - DriverStation.reportError("LaserCan " + canId + ": RegionOfInterest Configuration failed! " + e, true); - } + regionOfInterest = new RegionOfInterest(x, y, w, h); - return this; - } - - /** - * Sets the reigon of interest for the lasercan - * - * @param regionOfInterest The region of interest - * @return The current {@link LazyCan} instance - */ - public LazyCan withRegionOfInterest(RegionOfInterest regionOfInterest) { try { laserCan.setRegionOfInterest(regionOfInterest); } catch (ConfigurationFailedException e) { @@ -84,8 +77,8 @@ public LazyCan withRegionOfInterest(RegionOfInterest regionOfInterest) { /** * Sets the ranging mode of the LaserCan * - * @param rangingMode The ranging mode - * @return The current {@link LazyCan} instance + * @param rangingMode the new ranging mode + * @return the current LazyCan Object */ public LazyCan withRangingMode(RangingMode rangingMode) { try { @@ -93,15 +86,14 @@ public LazyCan withRangingMode(RangingMode rangingMode) { } catch (ConfigurationFailedException e) { System.out.println("LaserCan " + canId + ": RangingMode Configuration failed! " + e); } - return this; } /** * Sets the timing budget of the LaserCan * - * @param timingBudget The timing budget - * @return The current {@link LazyCan} instance + * @param timingBudget the new timing budget + * @return the current LazyCan Object */ public LazyCan withTimingBudget(TimingBudget timingBudget) { try { @@ -109,15 +101,14 @@ public LazyCan withTimingBudget(TimingBudget timingBudget) { } catch (ConfigurationFailedException e) { DriverStation.reportError("LaserCan " + canId + ": TimingBudget Configuration failed! " + e, true); } - return this; } /** * Sets the distance threshold of the LaserCan * - * @param threshold The threshold in milimeters - * @return The current {@link LazyCan} instance + * @param threshold the new threshold in milimeters + * @return the current LazyCan object */ public LazyCan withThreshold(double threshold) { this.threshold = threshold; diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index ff7cebe..45cd499 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -1,6 +1,8 @@ package raidzero.robot; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import java.util.ArrayList; @@ -109,7 +111,7 @@ public static class Joint { public static class Winch { public static final int MOTOR_ID = 16; - public static final double SPEED = 0.75; + public static final double SPEED = 1.0; } } @@ -127,9 +129,9 @@ public static enum REEFS { public static final List LEFT_REEF_WAYPOINTS = new ArrayList( List.of( - new Pose2d(3.70, 3.16, Rotation2d.fromDegrees(60)), // 17 Left + new Pose2d(3.735, 3.14, Rotation2d.fromDegrees(60)), // 17 Left new Pose2d(3.30, 4.15, Rotation2d.fromDegrees(0)), // 18 Left - new Pose2d(4.05, 5.1, Rotation2d.fromDegrees(300)), // 19 Left + new Pose2d(4.06, 5.105, Rotation2d.fromDegrees(300)), // 19 Left new Pose2d(5.2619, 4.99953, Rotation2d.fromDegrees(240)), // 20 Left new Pose2d(5.70, 3.85, Rotation2d.fromDegrees(180)), // 21 Left new Pose2d(4.9113, 2.93927, Rotation2d.fromDegrees(120)) // 22 Left @@ -140,39 +142,51 @@ public static enum REEFS { List.of( new Pose2d(4.05, 2.95, Rotation2d.fromDegrees(60)), // 17 Right new Pose2d(3.30, 3.85, Rotation2d.fromDegrees(0)), // 18 Right - new Pose2d(3.70, 4.89, Rotation2d.fromDegrees(300)), // 19 Right - new Pose2d(4.9419, 5.16453, Rotation2d.fromDegrees(240)), // 20 Right + new Pose2d(3.713, 4.925, Rotation2d.fromDegrees(300)), // 19 Right + new Pose2d(4.9489, 5.16, Rotation2d.fromDegrees(240)), // 20 Right new Pose2d(5.70, 4.20, Rotation2d.fromDegrees(180)), // 21 Right new Pose2d(5.2619, 3.05047, Rotation2d.fromDegrees(120)) // 22 Right ) ); + + public static final Pose2d BLUE_PROCESSOR = new Pose2d(5.987542, 0.78, Rotation2d.fromDegrees(90)); + public static final Pose2d RED_PROCESSOR = new Pose2d(17.55 - 5.987542, 8.05 - 0.78, Rotation2d.fromDegrees(180)); } public static class TelescopingArm { public static class Intake { public static final int MOTOR_ID = 12; - public static final int FOLLOW_ID = 13; - public static final double INTAKE_SPEED = 0.25; - public static final double INTAKE_LOWER_SPEED = 0.04; + public static final MotorArrangementValue MOTOR_ARRANGEMENT = MotorArrangementValue.Minion_JST; - public static final double SCOOCH_SPEED = 0.06; + public static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; - public static final double EXTAKE_SPEED = 0.1; - public static final double EXTAKE_TIMEOUT_S = 1.0; + public static final int STATOR_CURRENT_LIMIT = 30; + public static final int SUPPLY_CURRENT_LIMIT = 30; + public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; - public static final double LASERCAN_DISTANCE_THRESHOLD_MM = 50.0; + public static final double TOP_LASER_THRESHOLD_MM = 50.0; + public static final double BOTTOM_LASER_THRESHOLD_MM = 100.0; - public static final int CURRENT_LIMIT = 25; + public static final double INTAKE_SPEED = 0.85; + public static final double LOWER_SPEED = 0.25; + public static final double EJECT_SPEED = -0.80; + public static final double REVERSE_SPEED = -0.2; - public static final int BOTTOM_LASERCAN_ID = 0; - public static final int TOP_LASERCAN_ID = 1; + public static final double STALL_CURRENT_THRESHOLD = 20.0; + public static final double CURRENT_SPIKE_THRESHOLD = 10.0; + + public static final double EXTAKE_SPEED = 1.0; + public static final double EXTAKE_TIMEOUT_S = 1.0; - public static final int SERVO_HUB_ID = 3; + public static final double ALGAE_INTAKE_SPEED = 1.0; + public static final double ALGAE_EJECT_SPEED = -1.0; + public static final double HOLD_SPEED = 0.1; + + public static final double KP = 1.0; + public static final double KI = 0.0; + public static final double KD = 0.0; - public static final int SERVO_RETRACTED = 1950; - public static final int SERVO_EXTENDED = 1300; - public static final int SERVO_CENTER_WIDTH = 1625; } public static class Joint { @@ -180,7 +194,7 @@ public static class Joint { public static final int CANCODER_ID = 11; public static final double CANCODER_GEAR_RATIO = 28.0 / 80.0; - public static final double CANCODER_OFFSET = -(0.352783 - (0.25 / CANCODER_GEAR_RATIO)); + public static final double CANCODER_OFFSET = -(0.325684 - (0.25 / CANCODER_GEAR_RATIO)); public static final double CANCODER_DISCONTINUITY_POINT = 0.5; public static final double CONVERSION_FACTOR = (120.0 / 12.0) * 20.0; @@ -206,17 +220,21 @@ public static class Joint { } public static class Positions { - public static double[] L4_SCORING_POS_M = { -0.24, 2.65 }; - public static double[] L4_SCORING_POS_M_BLUE = { -0.17, 2.68 }; - public static double[] L4_CHECK_POSITION = { -0.25, 2.62 }; - public static double[] L4_GRAND_SLAM = { -0.2, 1.57 }; + public static final double[] L4_SCORING_POS_M = { -0.24, 2.75 }; + public static final double[] L4_SCORING_POS_M_BLUE = { -0.17, 2.68 }; + public static final double[] L4_CHECK_POSITION = { -0.25, 2.62 }; + public static final double[] L4_GRAND_SLAM = { -0.2, 1.57 }; + + public static final double[] L3_SCORING_POS_M = { -0.20, 1.57 }; + public static final double[] L2_SCORING_POS_M = { -0.15, 0.9 }; + public static final double[] L1_SCORING_POS_M = { 0.0, 0.0 }; - public static double[] L3_SCORING_POS_M = { -0.20, 1.57 }; - public static double[] L2_SCORING_POS_M = { -0.2, 0.9 }; - public static double[] L1_SCORING_POS_M = { 0.0, 0.0 }; + public static final double[] INTAKE_POS_M = { 0.5, 0.835 }; + public static final double[] INTAKE_POS_M_BLUE = { 0.5, 0.875 }; - public static double[] INTAKE_POS_M = { 0.5, 0.8425 }; - public static double[] INTAKE_POS_M_BLUE = { 0.5, 0.8425 }; + public static final double[] L3_ALGAE_POS_M = { 0.75, 1.3 }; + public static final double[] L2_ALGAE_POS_M = { 0.6, 0.7 }; + public static final double[] BARGE_SCORE_POS_M = { 0, 2.8 }; public static double[] HOME_POS_M = { 0.0, 0.0 }; } @@ -253,7 +271,9 @@ public static class Telescope { } } - public static final String CANIVORE_NAME = "CANdoAttitude"; + public static final String BASE_CANIVORE = "CANdoAttitude"; + public static final String KAYNE_BUS = "Kaynebus"; + public static final String RIO_BUS = "rio"; public static final double STICK_DEADBAND = 0.2; } \ No newline at end of file diff --git a/src/main/java/raidzero/robot/Robot.java b/src/main/java/raidzero/robot/Robot.java index 1a98712..a31817c 100644 --- a/src/main/java/raidzero/robot/Robot.java +++ b/src/main/java/raidzero/robot/Robot.java @@ -5,7 +5,6 @@ package raidzero.robot; import au.grapplerobotics.CanBridge; -import com.ctre.phoenix6.controls.StaticBrake; import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; @@ -24,12 +23,15 @@ public class Robot extends TimedRobot { private final RobotContainer m_robotContainer; + private boolean alreadyEnabled; + public Robot() { m_robotContainer = new RobotContainer(); CanBridge.runTCP(); WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); Elastic.selectTab("Setup"); + alreadyEnabled = false; } @Override @@ -44,14 +46,15 @@ public void disabledInit() {} @Override public void disabledPeriodic() { - Arm.system().updateCoastMode(); - - Swerve.system().initializeOtf(); + if (!alreadyEnabled) { + Arm.system().updateCoastMode(); + CoralIntake.system().updateCoastMode(); + Swerve.system().initializeOtf(); + } } @Override public void disabledExit() { - CoralIntake.system().getRoller().setControl(new StaticBrake()); ArmStrip.system().resetAnimation(); } @@ -64,6 +67,7 @@ public void autonomousInit() { } Elastic.selectTab("Autonomous"); + alreadyEnabled = true; } @Override diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index bb3ed0a..7d16418 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -14,13 +14,16 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathfindingCommand; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import raidzero.robot.Constants.TelescopingArm.Positions; import raidzero.robot.subsystems.LEDStrip.ArmStrip; import raidzero.robot.subsystems.climb.ClimbJoint; import raidzero.robot.subsystems.climb.Winch; @@ -95,12 +98,8 @@ private void configureBindings() { ) ); - // arm.setDefaultCommand(arm.moveArmWithDelay(Constants.TelescopingArm.Positions.INTAKE_POS_M)); - arm.setDefaultCommand(arm.moveToIntake()); coralIntake.setDefaultCommand(coralIntake.stop()); - // algaeIntake.setDefaultCommand(algaeIntake.moveJoint(Constants.AlgaeIntake.Joint.HOME_POSITION)); - climbJoint.setDefaultCommand(climbJoint.run(Constants.Climb.Joint.HOME_POS)); climbWinch.setDefaultCommand(climbWinch.stop()); @@ -146,7 +145,7 @@ private void configureBindings() { ); joystick.povDown().whileTrue( - arm.moveWithDelay(Constants.TelescopingArm.Positions.INTAKE_POS_M_BLUE) + swerve.pathToProcessor() ); // * Operator controls @@ -154,54 +153,70 @@ private void configureBindings() { operator.button(Constants.Bindings.BOTTOM_LEFT).onTrue(new InstantCommand(() -> arm.decreaseIntakeYOffset(-0.01), arm)); operator.button(Constants.Bindings.TOP_RIGHT).onTrue(new InstantCommand(() -> arm.removeIntakeOffset(), arm)); - operator.button(Constants.Bindings.L2).whileTrue( + operator.button(Constants.Bindings.L2).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveTo(Constants.TelescopingArm.Positions.L2_SCORING_POS_M) .onlyIf(swerve.isArmDeployable()) + .alongWith(new InstantCommand(() -> arm.checkDefaultCommand())) ); - operator.button(Constants.Bindings.L3).whileTrue( + operator.button(Constants.Bindings.L3).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveTo(Constants.TelescopingArm.Positions.L3_SCORING_POS_M) .onlyIf(swerve.isArmDeployable()) + .alongWith(new InstantCommand(() -> arm.checkDefaultCommand())) ); - operator.button(Constants.Bindings.L4).whileTrue( + operator.button(Constants.Bindings.L4).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveToL4() .onlyIf(swerve.isArmDeployable()) + .alongWith(new InstantCommand(() -> arm.checkDefaultCommand())) ); - operator.button(Constants.Bindings.ALGAE_INTAKE).onTrue(coralIntake.contingencyIntake()); - - operator.button(Constants.Bindings.CORAL_EXTAKE).whileTrue(coralIntake.extake()); + operator.button(Constants.Bindings.CORAL_EXTAKE).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()) + .whileTrue(coralIntake.extake()); operator.button(Constants.Bindings.CORAL_INTAKE).onTrue(coralIntake.intake()); - operator.button(Constants.Bindings.CORAL_SCOOCH).whileTrue(coralIntake.run(-Constants.TelescopingArm.Intake.SCOOCH_SPEED)); - - operator.button(Constants.Bindings.BOTTOM_RIGHT).onTrue(coralIntake.unstuckServo()); + operator.button(Constants.Bindings.CORAL_SCOOCH).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()) + .whileTrue(coralIntake.run(Constants.TelescopingArm.Intake.REVERSE_SPEED)); operator.button(Constants.Bindings.CLIMB_DEPLOY) .onTrue( Commands.waitSeconds(0.2) .andThen( - climbJoint.run(Constants.Climb.Joint.DEPLOYED_POS) - .until(() -> operator.button(Constants.Bindings.CLIMB_UP).getAsBoolean()) - .andThen(() -> climbJoint.stop()).alongWith( - new InstantCommand( - () -> climbJoint.setDeployedState() - ) - ) - ) + climbWinch.run(-1.0) + ).withTimeout(1.75) ); - operator.button(Constants.Bindings.CLIMB_DEPLOY).onTrue(arm.climbPos()); - - // operator.button(Constants.Bindings.CLIMB_UP) - // .whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED).onlyIf(climbJoint.isDeployed())); + operator.button(Constants.Bindings.CLIMB_DEPLOY) + .onTrue(arm.climbPos().alongWith(new InstantCommand(() -> climbJoint.setDeployedState()))); - operator.button(Constants.Bindings.CLIMB_UP).whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3)); - operator.button(Constants.Bindings.CLIMB_UP).onTrue(climbJoint.retract()); + operator.button(Constants.Bindings.CLIMB_UP).whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED)); operator.button(Constants.Bindings.CLIMB_DOWN) - .whileTrue(climbWinch.run(-Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3).onlyIf(climbJoint.isDeployed())); + .whileTrue(climbWinch.run(-Constants.Climb.Winch.SPEED)); + + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3).negate()) + .and(operator.button(Constants.Bindings.L2).negate()).and(operator.button(Constants.Bindings.L4).negate()) + .whileTrue(arm.moveWithRotations(0.25, 0)); + + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3).negate()) + .and(operator.button(Constants.Bindings.L2).negate()) + .whileTrue(coralIntake.holdAlgae()); - // operator.axisGreaterThan(0, 0.6).whileTrue(climbJoint.run(0.125)); - // operator.axisGreaterThan(1, 0.6).whileTrue(climbJoint.run(0.28)); + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3)) + .whileTrue( + arm.moveTo(Positions.L3_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae()) + ); + + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L2)) + .whileTrue( + arm.moveTo(Positions.L2_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae()) + ); + + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L4)) + .whileTrue(arm.moveTo(Positions.BARGE_SCORE_POS_M)); + + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.CORAL_EXTAKE)) + .whileTrue(coralIntake.extakeAlgae()); + + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.CORAL_SCOOCH)) + .whileTrue(coralIntake.run(-0.2)); swerve.registerTelemetry(logger::telemeterize); } @@ -252,12 +267,16 @@ private void registerPathplannerCommands() { NamedCommands.registerCommand( "ExtakeCoral", - coralIntake.run(0.1).until( - () -> { - return coralIntake.getBottomLaserDistance() >= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM && - coralIntake.getTopLaserDistance() >= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM; - } - ).withTimeout(1.0).andThen(() -> coralIntake.stop()) + coralIntake.run(Constants.TelescopingArm.Intake.EXTAKE_SPEED) + .onlyIf(() -> DriverStation.isAutonomousEnabled() && DriverStation.getMatchTime() >= 0.75) + .until( + () -> { + return coralIntake.getBottomLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && + coralIntake.getTopLaserDistance() >= Constants.TelescopingArm.Intake.BOTTOM_LASER_THRESHOLD_MM; + } + ) + .andThen(new WaitCommand(0.1)) + .andThen(() -> coralIntake.stop()) ); NamedCommands.registerCommand("IntakeCoral", coralIntake.intake().andThen(coralIntake.stop())); } diff --git a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java index 39d3ad3..855cb9b 100644 --- a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java +++ b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java @@ -22,7 +22,7 @@ public class ArmStrip implements Subsystem { private CANdle candle; private Arm arm; - private boolean armIsLegal, coralTooDown, coralTooUp, coralIsIn = false; + private boolean armIsLegal, coralTooUp, coralIsIn = false; private boolean strobeAlternate = false; private Timer strobeTimer = new Timer(); @@ -38,7 +38,7 @@ public class ArmStrip implements Subsystem { * Constructs a {@link ArmStrip} subsystem. */ private ArmStrip() { - this.candle = new CANdle(Constants.CANdle.CAN_ID, Constants.CANIVORE_NAME); + this.candle = new CANdle(Constants.CANdle.CAN_ID, Constants.BASE_CANIVORE); this.arm = Arm.system(); this.candle.configAllSettings(candleConfig()); @@ -80,14 +80,10 @@ private void updateStates() { armIsLegal = arm.getJointPosition() >= Constants.CANdle.ARM_JOINT_LOWER_BOUND && arm.getJointPosition() <= Constants.CANdle.ARM_JOINT_UPPER_BOUND; - coralTooDown = CoralIntake.system().getTopLaserDistance() > Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() < Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM; + coralTooUp = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && + CoralIntake.system().getBottomLaserDistance() > Constants.TelescopingArm.Intake.BOTTOM_LASER_THRESHOLD_MM; - coralTooUp = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() > Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM; - - coralIsIn = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() < Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM; + coralIsIn = CoralIntake.system().bottomLaserWithinThreshold(); } /** @@ -135,7 +131,7 @@ private void loopDisabled() { animation2Applied = false; animation3Applied = true; } - } else if (armIsLegal && ClimbJoint.system().getPosition() < 0.1 && !ClimbJoint.system().isDeployed().getAsBoolean()) { + } else if (armIsLegal && CoralIntake.system().getTopLaserDistance() < 10 && !ClimbJoint.system().isDeployed().getAsBoolean()) { if (!animation2Applied) { candle.clearAnimation(0); candle.clearAnimation(1); @@ -144,7 +140,7 @@ private void loopDisabled() { animationApplied = false; animation3Applied = false; } - } else if (!armIsLegal && ClimbJoint.system().getPosition() < 0.1 && !ClimbJoint.system().isDeployed().getAsBoolean()) { + } else if (!armIsLegal && CoralIntake.system().getTopLaserDistance() < 10 && !ClimbJoint.system().isDeployed().getAsBoolean()) { if (animationApplied || animation2Applied || animation3Applied) { candle.clearAnimation(0); candle.clearAnimation(1); @@ -216,16 +212,6 @@ private void loopAutonomous() { private void loopTeleop() { if (ClimbJoint.system().isDeployed().getAsBoolean()) { candle.animate(new StrobeAnimation(0, 0, 255, 0, 0.05, -1)); - } else if (coralTooDown) { - if (!animation2Applied) { - candle.clearAnimation(0); - candle.clearAnimation(1); - candle.animate(new ColorFlowAnimation(250, 160, 10, 0, 0.75, 25, Direction.Backward, 8), 0); - candle.animate(new ColorFlowAnimation(250, 160, 10, 0, 0.75, 27, Direction.Forward, 33), 1); - animationApplied = false; - animation2Applied = true; - animation3Applied = false; - } } else if (coralTooUp) { if (!animation2Applied) { candle.clearAnimation(0); diff --git a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java index a3f738e..4048e37 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java +++ b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java @@ -22,7 +22,7 @@ public class ClimbJoint extends SubsystemBase { * Constructs a {@link ClimbJoint} subsystem instance */ private ClimbJoint() { - joint = new TalonFX(Constants.Climb.Joint.MOTOR_ID); + joint = new TalonFX(Constants.Climb.Joint.MOTOR_ID, Constants.RIO_BUS); joint.getConfigurator().apply(jointConfiguration()); joint.setNeutralMode(NeutralModeValue.Brake); } @@ -39,6 +39,7 @@ public Command run(double setpoint) { /** * Retracts the joint and stops the motor once vertical + * * @return A {@link Command} that retracts the joint */ public Command retract() { @@ -88,7 +89,7 @@ public double getVelocity() { } /** - * Returns a {@link BooleanSupplier} that checks if the joint is deployed + * Checks if the joint is deployed * * @return A {@link BooleanSupplier} that checks if the joint is deployed */ diff --git a/src/main/java/raidzero/robot/subsystems/climb/Winch.java b/src/main/java/raidzero/robot/subsystems/climb/Winch.java index 2c584ec..15252b2 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/Winch.java +++ b/src/main/java/raidzero/robot/subsystems/climb/Winch.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.robot.Constants; @@ -18,7 +17,7 @@ public class Winch extends SubsystemBase { * Constructs a {@link Winch} subsystem instance */ private Winch() { - winch = new TalonFX(Constants.Climb.Winch.MOTOR_ID); + winch = new TalonFX(Constants.Climb.Winch.MOTOR_ID, Constants.RIO_BUS); winch.getConfigurator().apply(winchConfiguration()); winch.setNeutralMode(NeutralModeValue.Brake); } @@ -29,7 +28,7 @@ private Winch() { * @param speed The speed to run the winch at * @return A {@link Command} that runs the winch at the specified speed */ - public Command run(double speed, boolean ramp) { + public Command run(double speed) { return run(() -> winch.set(speed)); } @@ -51,7 +50,7 @@ private TalonFXConfiguration winchConfiguration() { TalonFXConfiguration configuration = new TalonFXConfiguration() .withOpenLoopRamps( new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(0.2) + .withDutyCycleOpenLoopRampPeriod(0.1) ); return configuration; diff --git a/src/main/java/raidzero/robot/subsystems/drivetrain/Swerve.java b/src/main/java/raidzero/robot/subsystems/drivetrain/Swerve.java index a8d1004..3e093b4 100644 --- a/src/main/java/raidzero/robot/subsystems/drivetrain/Swerve.java +++ b/src/main/java/raidzero/robot/subsystems/drivetrain/Swerve.java @@ -306,6 +306,25 @@ public Command pathToReef(Constants.Swerve.REEFS reef) { }); } + /** + * Moves the robot to the red or blue processor depending on current alliance + * + * @return A {@link DeferredCommand} that moves the robot to the red or blue processor + */ + public Command pathToProcessor() { + return defer(() -> { + Pose2d target = null; + + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { + target = Constants.Swerve.RED_PROCESSOR; + } else { + target = Constants.Swerve.BLUE_PROCESSOR; + } + + return goToPose(target).withTimeout(0.01).andThen(goToPose(target)); + }); + } + /** * Moves the robot to the nearest coral station * diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index 5805b5b..a252e05 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -34,15 +34,15 @@ public class Arm extends SubsystemBase { * Constructs an {@link Arm} subsystem instance */ private Arm() { - telescope = new TalonFX(Constants.TelescopingArm.Telescope.MOTOR_ID); + telescope = new TalonFX(Constants.TelescopingArm.Telescope.MOTOR_ID, "Kaynebus"); telescope.getConfigurator().apply(telescopeConfiguration()); telescope.setNeutralMode(NeutralModeValue.Brake); - joint = new TalonFX(Constants.TelescopingArm.Joint.MOTOR_ID); + joint = new TalonFX(Constants.TelescopingArm.Joint.MOTOR_ID, "Kaynebus"); joint.getConfigurator().apply(jointConfiguration()); joint.setNeutralMode(NeutralModeValue.Brake); - jointCANcoder = new CANcoder(Constants.TelescopingArm.Joint.CANCODER_ID); + jointCANcoder = new CANcoder(Constants.TelescopingArm.Joint.CANCODER_ID, "Kaynebus"); jointCANcoder.getConfigurator().apply(jointCANCoderConfiguration()); currentPose = new double[] { 0.0, 0.0 }; @@ -77,6 +77,19 @@ public Command moveTo(double[] desiredPosition) { } } + /** + * Moves the arm to the desired joint and telescope setpoints + * + * @param jointSetpoint The desired joint setpoint in rotations + * @param telescopeSetpoint The desired telescope setpoint in percentage of full range of motion + * @return A {@link Command} that moves the arm to the desired setpoints + */ + public Command moveWithRotations(double jointSetpoint, double telescopeSetpoint) { + return run( + () -> moveJoint(jointSetpoint) + ).alongWith(Commands.waitSeconds(0.3).andThen(() -> moveTelescope(telescopeSetpoint))); + } + /** * Moves the arm to the desired x and y setpoints without delay * @@ -117,12 +130,12 @@ public Command moveWithDelay(double[] desiredPosition) { } /** - * Decreases the intake Y offset by a desired amount + * Adjusts the intake Y offset by a desired amount * - * @param ammount The desired offset amount + * @param amount The desired offset amount */ - public void decreaseIntakeYOffset(double ammount) { - intakePosYOffset += ammount; + public void decreaseIntakeYOffset(double amount) { + intakePosYOffset += amount; } /** @@ -155,6 +168,7 @@ public Command moveToIntake() { ); } + } /** @@ -183,6 +197,15 @@ public Command climbPos() { }); } + /** + * Checks if a default command is present. If it isn't present, it will assign {@link Arm#moveToIntake()} as the default command. + */ + public void checkDefaultCommand() { + if (this.getDefaultCommand() == null) { + this.setDefaultCommand(this.moveToIntake()); + } + } + /** * Runs just the telescope to the supplied setpoint * @@ -224,7 +247,7 @@ public void updateCoastMode() { * @return True if the arm joint should be in coast mode, false otherwise */ private boolean shouldBeInCoast() { - return (ClimbJoint.system().getPosition() < Constants.CANdle.CLIMB_JOINT_THRESHOLD); + return CoralIntake.system().getTopLaserDistance() < 10; } /** @@ -311,6 +334,7 @@ public void stopAll() { public void periodic() { SmartDashboard.putNumber("Elevator pos", getTelescopePosition()); SmartDashboard.putNumber("Intake Y Offset", Math.round(intakePosYOffset * 100) / 100.0); + SmartDashboard.putBoolean("DefaultCommand", this.getDefaultCommand() != null); } /** diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 72eac00..1234589 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -2,18 +2,12 @@ import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFXS; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.servohub.ServoChannel; -import com.revrobotics.servohub.ServoChannel.ChannelId; -import com.revrobotics.servohub.ServoHub; -import com.revrobotics.servohub.config.ServoChannelConfig.BehaviorWhenDisabled; -import com.revrobotics.servohub.config.ServoHubConfig; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.lib.LazyCan; @@ -21,11 +15,7 @@ import raidzero.robot.Constants.TelescopingArm.Intake; public class CoralIntake extends SubsystemBase { - private TalonFXS roller, follow; - - private ServoHub servoHub; - private ServoChannel intakeBlock; - + private TalonFXS roller, follower; private LazyCan bottomLaser, topLaser; private static CoralIntake system; @@ -34,90 +24,93 @@ public class CoralIntake extends SubsystemBase { * Constructs a {@link CoralIntake} subsystem instance */ private CoralIntake() { - roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID); + roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID, Constants.RIO_BUS); roller.getConfigurator().apply(rollerConfiguration()); - follow = new TalonFXS(Constants.TelescopingArm.Intake.FOLLOW_ID); - follow.setControl(new Follower(Constants.TelescopingArm.Intake.MOTOR_ID, true)); - follow.getConfigurator().apply(followConfiguration()); + follower = new TalonFXS(13); + follower.getConfigurator().apply(followerConfiguration()); + follower.setControl(new Follower(Intake.MOTOR_ID, false)); - bottomLaser = new LazyCan(Constants.TelescopingArm.Intake.BOTTOM_LASERCAN_ID) - .withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 8, 4, 4) - .withTimingBudget(TimingBudget.TIMING_BUDGET_20MS); + bottomLaser = new LazyCan(1).withRangingMode(RangingMode.SHORT) + .withRegionOfInterest(14, 8, 16, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + .withThreshold(Intake.BOTTOM_LASER_THRESHOLD_MM); - topLaser = new LazyCan(Constants.TelescopingArm.Intake.TOP_LASERCAN_ID) - .withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 8, 4, 4) - .withTimingBudget(TimingBudget.TIMING_BUDGET_20MS); - - servoHub = new ServoHub(Constants.TelescopingArm.Intake.SERVO_HUB_ID); - servoHub.configure(getServoHubConfig(), ServoHub.ResetMode.kResetSafeParameters); + topLaser = new LazyCan(0).withRangingMode(RangingMode.LONG) + .withRegionOfInterest(8, 14, 16, 4).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + .withThreshold(Intake.TOP_LASER_THRESHOLD_MM); + } - intakeBlock = servoHub.getServoChannel(ChannelId.kChannelId2); - intakeBlock.setPowered(true); - intakeBlock.setEnabled(true); + /** + * Intakes a coral + * + * @return A {@link Command} that intakes a coral + */ + public Command intake() { + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()); } /** - * Gets the roller motor controller for disabled init to check for position + * Intakes a coral until the top laser is triggered + * + *

Note: This method should only be used during the autonomous period.

* - * @return The Roller motor + * @return A {@link Command} that intakes a coral until the top laser is triggered */ - public TalonFXS getRoller() { - return roller; + public Command autoIntakeP1() { + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> topLaser.withinThreshold()); } /** - * Creates a {@link Command} to intake the coral + * Intakes a coral until the bottom laser is triggered + * + *

Note: This method should only be used during the autonomous period.

* - * @return A {@link Command} to intake the coral + * @return A {@link Command} that intakes a coral until the bottom laser is triggered */ - public Command intake() { - return run(() -> roller.set(Constants.TelescopingArm.Intake.INTAKE_SPEED)) - .until(() -> getBottomLaserDistance() <= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM); + public Command autoIntakeP2() { + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()); + } + + /** + * Intakes an lgae + + * @return A {@link Command} that intakes an algae + */ + public Command intakeAlgae() { + return run(() -> roller.set(Intake.INTAKE_SPEED)); } /** - * Creates a {@link Command} to intake the coral slower + * Extakes an algae * - * @return A {@link Command} to intake the coral slower + * @return A {@link Command} that extakes an algae */ - public Command contingencyIntake() { - return startRun( - () -> roller.getConfigurator().apply( - rollerConfiguration().withCurrentLimits( - new CurrentLimitsConfigs().withSupplyCurrentLimit(3).withSupplyCurrentLowerLimit(1.5).withSupplyCurrentLowerTime(0.05) - ) - ), - () -> roller.set(Constants.TelescopingArm.Intake.INTAKE_SPEED - 0.05) - ).until(() -> getBottomLaserDistance() <= Intake.LASERCAN_DISTANCE_THRESHOLD_MM) - .finallyDo(() -> roller.getConfigurator().apply(rollerConfiguration())); + public Command extakeAlgae() { + return run(() -> roller.set(Intake.ALGAE_EJECT_SPEED)); } /** - * Creates a {@link Command} to move the coral upwards to unstuck the servo block + * Holds the algae by applying a small amount of voltage * - * @return A {@link Command} to move the coral upwards + * @return A {@link Command} that holds the algae */ - public Command unstuckServo() { - return run(() -> roller.set(-Constants.TelescopingArm.Intake.INTAKE_LOWER_SPEED)) - .until(() -> getBottomLaserDistance() >= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM); + public Command holdAlgae() { + return run(() -> roller.set(Intake.HOLD_SPEED)); } /** - * Creates a {@link Command} to stop the intake + * Stops the intake * - * @return A {@link Command} to stop the intake + * @return A {@link Command} that stops the intake */ public Command stop() { return runOnce(() -> roller.stopMotor()); } /** - * Creates a {@link Command} to extake the coral + * Extakes a coral * - * @return A {@link Command} to extake the coral + * @return A {@link Command} that extakes a coral */ public Command extake() { return run(() -> roller.set(Constants.TelescopingArm.Intake.EXTAKE_SPEED)) @@ -125,7 +118,7 @@ public Command extake() { } /** - * Creates a {@link Command} to run the roller at the specified speed + * Runs the roller at the specified speed * * @param speed The speed to run the roller at [-1, 1] * @return A {@link Command} to run the roller at the specified speed @@ -134,15 +127,30 @@ public Command run(double speed) { return run(() -> roller.set(speed)); } - @Override - public void periodic() { - if (getBottomLaserDistance() > Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM) { - intakeBlock.setPulseWidth(Constants.TelescopingArm.Intake.SERVO_EXTENDED); + /** + * Updates the coast mode of the roller and follower motors + * + *

Note: This should only be called during disabled.

+ */ + public void updateCoastMode() { + if (shouldBeInCoast()) { + roller.setNeutralMode(NeutralModeValue.Coast); + follower.setNeutralMode(NeutralModeValue.Coast); } else { - intakeBlock.setPulseWidth(Constants.TelescopingArm.Intake.SERVO_RETRACTED); + roller.setNeutralMode(NeutralModeValue.Brake); + follower.setNeutralMode(NeutralModeValue.Brake); } } + /** + * Checks if the roller should be in coast mode + * + * @return True if the roller should be in coast mode, false otherwise + */ + private boolean shouldBeInCoast() { + return getTopLaserDistance() < 10; + } + /** * Gets the distance from the LaserCAN * @@ -152,6 +160,24 @@ public int getTopLaserDistance() { return topLaser.getDistanceMm(); } + /** + * Checks if the top laser is within the threshold + * + * @return True if the top laser is within the threshold, false otherwise + */ + public boolean topLaserWithinThreshold() { + return topLaser.withinThreshold(); + } + + /** + * Checks if the bottom laser is within the threshold + * + * @return True if the bottom laser is within the threshold, false otherwise + */ + public boolean bottomLaserWithinThreshold() { + return bottomLaser.withinThreshold(); + } + /** * Gets the distance from the LaserCAN * @@ -161,6 +187,14 @@ public int getBottomLaserDistance() { return bottomLaser.getDistanceMm(); } + @Override + public void periodic() { + SmartDashboard.putNumber("Top laser mm", getTopLaserDistance()); + SmartDashboard.putNumber("Bottom laser mm", getBottomLaserDistance()); + SmartDashboard.putBoolean("Top laser", topLaserWithinThreshold()); + SmartDashboard.putBoolean("Bottom laser", bottomLaserWithinThreshold()); + } + /** * Gets the {@link TalonFXSConfiguration} for the roller motor * @@ -169,8 +203,14 @@ public int getBottomLaserDistance() { private TalonFXSConfiguration rollerConfiguration() { TalonFXSConfiguration configuration = new TalonFXSConfiguration(); - configuration.Commutation.MotorArrangement = MotorArrangementValue.Minion_JST; - configuration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + configuration.Commutation.MotorArrangement = Intake.MOTOR_ARRANGEMENT; + configuration.MotorOutput.Inverted = Intake.INVERTED_VALUE; + + configuration.CurrentLimits.StatorCurrentLimit = Intake.STATOR_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLimit = Intake.SUPPLY_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLowerTime = Intake.SUPPLY_CURRENT_LOWER_TIME; + + configuration.Slot0 = new Slot0Configs().withKP(Intake.KP).withKI(Intake.KI).withKD(Intake.KD); configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -178,37 +218,25 @@ private TalonFXSConfiguration rollerConfiguration() { } /** - * Gets the {@link TalonFXSConfiguration} for the roller follower + * Gets the {@link TalonFXSConfiguration} for the follower motor * - * @return The {@link TalonFXSConfiguration} for the roller follower + * @return The {@link TalonFXSConfiguration} for the follower motor */ - private TalonFXSConfiguration followConfiguration() { + private TalonFXSConfiguration followerConfiguration() { TalonFXSConfiguration configuration = new TalonFXSConfiguration(); - configuration.Commutation.MotorArrangement = MotorArrangementValue.Minion_JST; + configuration.Commutation.MotorArrangement = Intake.MOTOR_ARRANGEMENT; + configuration.MotorOutput.Inverted = Intake.INVERTED_VALUE; - configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - return configuration; - } + configuration.CurrentLimits.StatorCurrentLimit = Intake.STATOR_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLimit = Intake.SUPPLY_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLowerTime = Intake.SUPPLY_CURRENT_LOWER_TIME; - /** - * Gets the {@link ServoHubConfig} for the REV Servo Hub - * - * @return The {@link ServoHubConfig} for the REV Servo Hub - */ - private ServoHubConfig getServoHubConfig() { - ServoHubConfig config = new ServoHubConfig(); + configuration.Slot0 = new Slot0Configs().withKP(Intake.KP).withKI(Intake.KI).withKD(Intake.KD); - config.channel2 - .pulseRange( - Constants.TelescopingArm.Intake.SERVO_EXTENDED, - Constants.TelescopingArm.Intake.SERVO_CENTER_WIDTH, - Constants.TelescopingArm.Intake.SERVO_RETRACTED - ) - .disableBehavior(BehaviorWhenDisabled.kSupplyPower); + configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - return config; + return configuration; } /**