diff --git a/src/main/java/frc/robot/Commands/TeleopCommands/PathfindingCommands.java b/src/main/java/frc/robot/Commands/TeleopCommands/PathfindingCommands.java index 6a0682f..26d7fcb 100644 --- a/src/main/java/frc/robot/Commands/TeleopCommands/PathfindingCommands.java +++ b/src/main/java/frc/robot/Commands/TeleopCommands/PathfindingCommands.java @@ -24,14 +24,14 @@ public class PathfindingCommands { * move again. The trajectory will automatically be rotated to the Red alliance. * *

Since a new trajectory is meant to be generated upon every button press, all the code must - * be inside of the return. This is done by returning a Commands.run() with a block of code inside - * of the lambda function for the {@link Runnable} parameter. + * be inside of the return. This is done by returning a {@code Commands.run()} with a block of + * code inside of the lambda function for the {@link Runnable} parameter. * - * @param drive Drive subsystem - * @param vision Vision subsystem + * @param drive {@link Drive} subsystem + * @param vision {@link Vision} subsystem * @param distanceFromTagMeters Distance in front of the AprilTag for the robot to end up. - * @param stopTrigger {@link BooleanSupplier} with the condition to end the Pathfinding command - * @return Command that makes the robot follow a trajectory to in front of the AprilTag. + * @param stopTrigger {@link BooleanSupplier} with the condition to end the Pathfinding command. + * @return {@link Command} that makes the robot follow a trajectory to in front of the AprilTag. */ public static Command pathfindToCurrentTag( Drive drive, @@ -40,7 +40,7 @@ public static Command pathfindToCurrentTag( BooleanSupplier stopTrigger) { return Commands.run( () -> { - /** + /* * Get ID of AprilTag currently seen by the front camera, if any. If an invalid ID is * given the apriltagPose Optional will be empty */ @@ -51,20 +51,19 @@ public static Command pathfindToCurrentTag( // If no valid tag returned then return a print messsage instead if (apriltagPose.isEmpty()) { Commands.print("Invalid AprilTag ID").until(stopTrigger).schedule(); - ; } else { // Turn 3d AprilTag pose into a 2d pose var apriltagPose2d = apriltagPose.get().toPose2d(); - /** + /* * The goal pose is the end position for the center of the robot. Transforming by half * the track width will leave the robot right up against the tag and any additional * distance can be added */ var goalPose = new Pose2d( - /** + /* * Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to * robot) is the desired distance away from the tag */ @@ -89,23 +88,23 @@ public static Command pathfindToCurrentTag( /** * Generates a trajectory for the robot to follow to the AprilTag corresponding to the ID inputed * with an additional distance translation. The trajectory will automatically be rotated to the - * Red alliance. + * red alliance. * * @param tagID AprilTag ID of the desired AprilTag to align to. * @param wallDistanceMeters Distance in front of the AprilTag for the robot to end up. - * @return Command that makes the robot follow a trajectory to in front of the AprilTag. + * @return {@link Command} that makes the robot follow a trajectory to in front of the AprilTag. */ public static Command pathfindToAprilTag(IntSupplier tagID, DoubleSupplier wallDistanceMeters) { // Get the 2d pose of the AprilTag associated with the inputed ID var apriltagPose = FieldConstants.APRILTAG_FIELD_LAYOUT.getTagPose(tagID.getAsInt()).get().toPose2d(); - /** + /* * The goal pose is the end position for the center of the robot. Transforming by half the track * width will leave the robot right up against the tag and any additional distance can be added */ var goalPose = new Pose2d( - /** + /* * Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to robot) * is the desired distance away from the tag */ @@ -124,11 +123,11 @@ public static Command pathfindToAprilTag(IntSupplier tagID, DoubleSupplier wallD /** * Generates a trajectory for the robot to follow to a specified REEF BRANCH with an additional - * distance translation. The trajectory will automatically be rotated to the Red alliance. + * distance translation. The trajectory will automatically be rotated to the red alliance. * * @param branchLetter Letter corresponding to BRANCH to pathfind to. * @param wallDistanceMeters Distance from the REEF wall in meters. - * @return Command that makes the robot follow a trajectory to in front of the BRANCH. + * @return {@link Command} that makes the robot follow a trajectory to in front of the BRANCH. */ public static Command pathfindToBranch(String branchLetter, DoubleSupplier wallDistanceMeters) { // Position of BRANCH corresponding to zone the robot is in @@ -138,10 +137,8 @@ public static Command pathfindToBranch(String branchLetter, DoubleSupplier wallD // the BRANCH var goalPose = new Pose2d( - /** - * Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to robot) - * is the desired distance away from the tag - */ + // Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to robot) + // is the desired distance away from the tag branchPose.getX() + ((DriveConstants.TRACK_WIDTH_M / 2) + FieldConstants.BRANCH_TO_WALL_X_M @@ -164,13 +161,14 @@ public static Command pathfindToBranch(String branchLetter, DoubleSupplier wallD * automatically be rotated to the Red alliance. * *

Since a new trajectory is meant to be generated upon every button press, all the code must - * be inside of the return. This is done by returning a Commands.run() with a block of code inside - * of the lambda function for the {@link Runnable} parameter. + * be inside of the return. This is done by returning a {@code Commands.run()} with a block of + * code inside of the lambda function for the {@link Runnable} parameter. * - * @param drive Drive subsystem + * @param drive {@link Drive} subsystem * @param wallDistanceMeters Distance from the REEF wall in meters. - * @param stopTrigger {@link BooleanSupplier} with the condition to end the Pathfinding command - * @return Command that makes the robot follow a trajectory to in front of the nearest BRANCH. + * @param stopTrigger {@link BooleanSupplier} with the condition to end the Pathfinding command. + * @return {@link Command} that makes the robot follow a trajectory to in front of the nearest + * BRANCH. */ public static Command pathfindToClosestReef( Drive drive, DoubleSupplier wallDistanceMeters, BooleanSupplier stopTrigger) { @@ -241,13 +239,13 @@ public static Command pathfindToClosestReef( } // Position of BRANCH corresponding to zone the robot is in var branchPose = FieldConstants.BRANCH_POSES.get(branchLetter); - - // Translated pose to send to Pathfinder, so that robot isn't commanded to go directly on - // top of - // the BRANCH + /* + * Translated pose to send to Pathfinder, so that robot isn't commanded to go directly on + * top of the BRANCH + */ var goalPose = new Pose2d( - /** + /* * Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to * robot) is the desired distance away from the tag */ @@ -261,16 +259,16 @@ public static Command pathfindToClosestReef( + FieldConstants.BRANCH_TO_WALL_X_M + wallDistanceMeters.getAsDouble()) * branchPose.getRotation().getSin(), - // Rotate by 180 as the AprilTag angles are rotated 180 degrees relative to the - // robot + /* + * Rotate by 180 as the AprilTag angles are rotated 180 degrees relative to the + * robot + */ branchPose.getRotation().plus(Rotation2d.k180deg)); // Create and follow the trajectory to the goal pose AutoBuilder.pathfindToPoseFlipped( goalPose, PathPlannerConstants.DEFAULT_PATH_CONSTRAINTS, 0) .until(stopTrigger) - .alongWith( - Commands.print("Angle to REEF: " + thetaDeg + "\nBRANCH Letter: " + branchLetter)) .schedule(); }, drive); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b00d689..96e75f6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -175,8 +175,9 @@ public static Optional getAprilTagPose(int ID) { /** Angle from AprilTag to BRANCH that the hypotenuse makes */ double ARPILTAG_TO_BRANCH_ANGLE_RAD = Units.degreesToRadians(30); + // Initialize BRANCH poses for (int i = 0; i < 6; i++) { - // Left BRANCH of face + // Left BRANCH of REEF face var leftBranch = new Pose2d( CENTER_FACES[i].getX() @@ -191,7 +192,7 @@ public static Optional getAprilTagPose(int ID) { + CENTER_FACES[i].getRotation().getRadians())), CENTER_FACES[i].getRotation()); - // Right BRANCH of face + // Right BRANCH of REEF face var rightBranch = new Pose2d( CENTER_FACES[i].getX() @@ -206,6 +207,7 @@ public static Optional getAprilTagPose(int ID) { + CENTER_FACES[i].getRotation().getRadians())), CENTER_FACES[i].getRotation()); + // Map poses to corresponding BRANCH letter BRANCH_POSES.put(BRANCH_LETTERS.substring(i, i + 1), leftBranch); BRANCH_POSES.put(BRANCH_LETTERS.substring(i + 6, i + 7), rightBranch); } @@ -245,7 +247,7 @@ public final class PathPlannerConstants { public static final PathConstraints DEFAULT_PATH_CONSTRAINTS = new PathConstraints( 5.2, 5.2, Units.degreesToRadians(515.65), Units.degreesToRadians(262.82)); - /** Default distnace away from an AprilTag the robot should be when Pathfinding to it */ - public static final double DEFAULT_APRILTAG_DISTANCE_M = Units.inchesToMeters(6); + /** Default distance away from any wall when the robot is Pathfinding towards one */ + public static final double DEFAULT_WALL_DISTANCE_M = Units.inchesToMeters(6); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b0627db..a29ec10 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -86,7 +86,7 @@ public Robot() { * 3: Commit number (of this branch), * 4: Functionality: 0 = working, 1 = WIP, 2 = doesn't work */ - SmartDashboard.putString("Version Number", "23.41.28.1"); + SmartDashboard.putString("Version Number", "24.0.24.0"); SmartDashboard.putString("Last Deployed: ", BuildConstants.BUILD_DATE); // Run a warmup command for the Pathfinder because the first command can potentially have a diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2ba7414..f417d04 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,27 +8,53 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Commands.TeleopCommands.DriveCommands; import frc.robot.Commands.TeleopCommands.PathfindingCommands; -import frc.robot.Constants.FieldConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.PathPlannerConstants; import frc.robot.Constants.RobotStateConstants; +import frc.robot.Subsystems.Algae.EndEffector.AEE; +import frc.robot.Subsystems.Algae.EndEffector.AEEIO; +import frc.robot.Subsystems.Algae.EndEffector.AEEIOSim; +import frc.robot.Subsystems.Algae.EndEffector.AEEIOSparkMax; +import frc.robot.Subsystems.Algae.Pivot.AlgaePivot; +import frc.robot.Subsystems.Algae.Pivot.AlgaePivotConstants; +import frc.robot.Subsystems.Algae.Pivot.AlgaePivotIO; +import frc.robot.Subsystems.Algae.Pivot.AlgaePivotIOSim; +import frc.robot.Subsystems.Algae.Pivot.AlgaePivotIOSparkMax; +import frc.robot.Subsystems.Climber.Climber; +import frc.robot.Subsystems.Climber.ClimberConstants; +import frc.robot.Subsystems.Climber.ClimberIO; +import frc.robot.Subsystems.Climber.ClimberIOSim; +import frc.robot.Subsystems.Climber.ClimberIOTalonFX; +import frc.robot.Subsystems.CoralEndEffector.CEE; +import frc.robot.Subsystems.CoralEndEffector.CEEIO; +import frc.robot.Subsystems.CoralEndEffector.CEEIOSim; +import frc.robot.Subsystems.CoralEndEffector.CEEIOSparkMax; import frc.robot.Subsystems.Drive.Drive; import frc.robot.Subsystems.Drive.ModuleIO; import frc.robot.Subsystems.Drive.ModuleIOSim; import frc.robot.Subsystems.Drive.ModuleIOSparkMaxTalonFX; +import frc.robot.Subsystems.Funnel.Funnel; +import frc.robot.Subsystems.Funnel.FunnelIO; +import frc.robot.Subsystems.Funnel.FunnelIOSim; +import frc.robot.Subsystems.Funnel.FunnelIOSparkMax; import frc.robot.Subsystems.Gyro.Gyro; import frc.robot.Subsystems.Gyro.GyroIO; import frc.robot.Subsystems.Gyro.GyroIOPigeon2; +import frc.robot.Subsystems.Periscope.Periscope; +import frc.robot.Subsystems.Periscope.PeriscopeConstants; +import frc.robot.Subsystems.Periscope.PeriscopeIO; +import frc.robot.Subsystems.Periscope.PeriscopeIOSim; +import frc.robot.Subsystems.Periscope.PeriscopeIOTalonFX; import frc.robot.Subsystems.Vision.Vision; import frc.robot.Subsystems.Vision.VisionConstants; import frc.robot.Subsystems.Vision.VisionIO; import frc.robot.Subsystems.Vision.VisionIOPhotonVision; import frc.robot.Subsystems.Vision.VisionIOSim; -import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { @@ -38,12 +64,12 @@ public class RobotContainer { private final Gyro m_gyroSubsystem; // Mechanisms - // private final AlgaePivot m_algaePivotSubsystem; - // private final Periscope m_periscopeSubsystem; - // private final Climber m_climberSubsystem; - // private final Funnel m_funnelSubsystem; - // private final AEE m_AEESubsystem; - // private final CEE m_CEESubsystem; + private final AlgaePivot m_algaePivotSubsystem; + private final Periscope m_periscopeSubsystem; + private final Climber m_climberSubsystem; + private final Funnel m_funnelSubsystem; + private final AEE m_AEESubsystem; + private final CEE m_CEESubsystem; // Utils private final Vision m_visionSubsystem; @@ -71,12 +97,12 @@ public RobotContainer() { new ModuleIOSparkMaxTalonFX(2), new ModuleIOSparkMaxTalonFX(3), m_gyroSubsystem); - // m_algaePivotSubsystem = new AlgaePivot(new AlgaePivotIOSparkMax()); - // m_periscopeSubsystem = new Periscope(new PeriscopeIOTalonFX()); - // m_climberSubsystem = new Climber(new ClimberIOTalonFX()); - // m_funnelSubsystem = new Funnel(new FunnelIOSparkMax()); - // m_AEESubsystem = new AEE(new AEEIOSparkMax() {}); - // m_CEESubsystem = new CEE(new CEEIOSparkMax()); + m_algaePivotSubsystem = new AlgaePivot(new AlgaePivotIOSparkMax()); + m_periscopeSubsystem = new Periscope(new PeriscopeIOTalonFX()); + m_climberSubsystem = new Climber(new ClimberIOTalonFX()); + m_funnelSubsystem = new Funnel(new FunnelIOSparkMax()); + m_AEESubsystem = new AEE(new AEEIOSparkMax() {}); + m_CEESubsystem = new CEE(new CEEIOSparkMax()); m_visionSubsystem = new Vision( m_driveSubsystem::addVisionMeasurement, @@ -94,12 +120,12 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), m_gyroSubsystem); - // m_algaePivotSubsystem = new AlgaePivot(new AlgaePivotIOSim()); - // m_periscopeSubsystem = new Periscope(new PeriscopeIOSim()); - // m_climberSubsystem = new Climber(new ClimberIOSim()); - // m_funnelSubsystem = new Funnel(new FunnelIOSim()); - // m_AEESubsystem = new AEE(new AEEIOSim() {}); - // m_CEESubsystem = new CEE(new CEEIOSim()); + m_algaePivotSubsystem = new AlgaePivot(new AlgaePivotIOSim()); + m_periscopeSubsystem = new Periscope(new PeriscopeIOSim()); + m_climberSubsystem = new Climber(new ClimberIOSim()); + m_funnelSubsystem = new Funnel(new FunnelIOSim()); + m_AEESubsystem = new AEE(new AEEIOSim() {}); + m_CEESubsystem = new CEE(new CEEIOSim()); m_visionSubsystem = new Vision( m_driveSubsystem::addVisionMeasurement, @@ -118,12 +144,12 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, m_gyroSubsystem); - // m_algaePivotSubsystem = new AlgaePivot(new AlgaePivotIO() {}); - // m_periscopeSubsystem = new Periscope(new PeriscopeIO() {}); - // m_climberSubsystem = new Climber(new ClimberIO() {}); - // m_funnelSubsystem = new Funnel(new FunnelIO() {}); - // m_AEESubsystem = new AEE(new AEEIO() {}); - // m_CEESubsystem = new CEE(new CEEIO() {}); + m_algaePivotSubsystem = new AlgaePivot(new AlgaePivotIO() {}); + m_periscopeSubsystem = new Periscope(new PeriscopeIO() {}); + m_climberSubsystem = new Climber(new ClimberIO() {}); + m_funnelSubsystem = new Funnel(new FunnelIO() {}); + m_AEESubsystem = new AEE(new AEEIO() {}); + m_CEESubsystem = new CEE(new CEEIO() {}); m_visionSubsystem = new Vision(m_driveSubsystem::addVisionMeasurement, new VisionIO() {}); break; } @@ -153,20 +179,6 @@ public RobotContainer() { // Configure the button bindings configureButtonBindings(); - - // Testing if the BRANCH locations work // TODO: removed before PR'ing - Logger.recordOutput("Branches/A", FieldConstants.BRANCH_POSES.get("A")); - Logger.recordOutput("Branches/B", FieldConstants.BRANCH_POSES.get("B")); - Logger.recordOutput("Branches/C", FieldConstants.BRANCH_POSES.get("C")); - Logger.recordOutput("Branches/D", FieldConstants.BRANCH_POSES.get("D")); - Logger.recordOutput("Branches/E", FieldConstants.BRANCH_POSES.get("E")); - Logger.recordOutput("Branches/F", FieldConstants.BRANCH_POSES.get("F")); - Logger.recordOutput("Branches/G", FieldConstants.BRANCH_POSES.get("G")); - Logger.recordOutput("Branches/H", FieldConstants.BRANCH_POSES.get("H")); - Logger.recordOutput("Branches/I", FieldConstants.BRANCH_POSES.get("I")); - Logger.recordOutput("Branches/J", FieldConstants.BRANCH_POSES.get("J")); - Logger.recordOutput("Branches/K", FieldConstants.BRANCH_POSES.get("K")); - Logger.recordOutput("Branches/L", FieldConstants.BRANCH_POSES.get("L")); } /** @@ -263,28 +275,28 @@ private void driverControllerBindings() { PathfindingCommands.pathfindToCurrentTag( m_driveSubsystem, m_visionSubsystem, - () -> PathPlannerConstants.DEFAULT_APRILTAG_DISTANCE_M, + () -> PathPlannerConstants.DEFAULT_WALL_DISTANCE_M, m_driverController.x().negate())); // AprilTag 18 - REEF m_driverController .leftTrigger() .onTrue( PathfindingCommands.pathfindToAprilTag( - () -> 18, () -> PathPlannerConstants.DEFAULT_APRILTAG_DISTANCE_M) + () -> 18, () -> PathPlannerConstants.DEFAULT_WALL_DISTANCE_M) .until(m_driverController.leftTrigger().negate())); // AprilTag 17 - REEF m_driverController .leftBumper() .onTrue( PathfindingCommands.pathfindToAprilTag( - () -> 17, () -> PathPlannerConstants.DEFAULT_APRILTAG_DISTANCE_M) + () -> 17, () -> PathPlannerConstants.DEFAULT_WALL_DISTANCE_M) .until(m_driverController.leftBumper().negate())); // AprilTag 19 - REEF m_driverController .rightTrigger() .onTrue( PathfindingCommands.pathfindToAprilTag( - () -> 19, () -> PathPlannerConstants.DEFAULT_APRILTAG_DISTANCE_M) + () -> 19, () -> PathPlannerConstants.DEFAULT_WALL_DISTANCE_M) .until(m_driverController.rightTrigger().negate())); // Closest REEF BRANCH m_driverController @@ -292,128 +304,128 @@ private void driverControllerBindings() { .onTrue( PathfindingCommands.pathfindToClosestReef( m_driveSubsystem, - () -> Units.inchesToMeters(12), + () -> PathPlannerConstants.DEFAULT_WALL_DISTANCE_M, m_driverController.rightBumper().negate())); } /** Aux Controls */ public void auxControllerBindings() { - // // AEE testing binding - // m_AEESubsystem.setDefaultCommand( - // new InstantCommand( - // () -> - // m_AEESubsystem.setVoltage( - // m_auxController.getLeftTriggerAxis() * RobotStateConstants.MAX_VOLTAGE), - // m_AEESubsystem)); - // m_auxController - // .leftBumper() - // .onTrue( - // Commands.run( - // () -> { - // m_AEESubsystem.enablePID(true); - // m_AEESubsystem.setSetpoint(Units.rotationsPerMinuteToRadiansPerSecond(1000)); - // }, - // m_AEESubsystem)) - // .onFalse( - // new InstantCommand( - // () -> { - // m_AEESubsystem.setSetpoint(0); - // m_AEESubsystem.enablePID(false); - // }, - // m_AEESubsystem)); + // AEE testing binding + m_AEESubsystem.setDefaultCommand( + new InstantCommand( + () -> + m_AEESubsystem.setVoltage( + m_auxController.getLeftTriggerAxis() * RobotStateConstants.MAX_VOLTAGE), + m_AEESubsystem)); + m_auxController + .leftBumper() + .onTrue( + Commands.run( + () -> { + m_AEESubsystem.enablePID(true); + m_AEESubsystem.setSetpoint(Units.rotationsPerMinuteToRadiansPerSecond(1000)); + }, + m_AEESubsystem)) + .onFalse( + new InstantCommand( + () -> { + m_AEESubsystem.setSetpoint(0); + m_AEESubsystem.enablePID(false); + }, + m_AEESubsystem)); - // // CEE testing binding - // m_CEESubsystem.setDefaultCommand( - // new InstantCommand( - // () -> - // m_CEESubsystem.setVoltage( - // m_auxController.getRightTriggerAxis() * RobotStateConstants.MAX_VOLTAGE), - // m_CEESubsystem)); - // m_auxController - // .rightBumper() - // .onTrue( - // Commands.run( - // () -> { - // m_CEESubsystem.enablePID(true); - // m_CEESubsystem.setSetpoint(Units.rotationsPerMinuteToRadiansPerSecond(1000)); - // }, - // m_CEESubsystem)) - // .onFalse( - // new InstantCommand( - // () -> { - // m_CEESubsystem.setSetpoint(0); - // m_CEESubsystem.enablePID(false); - // }, - // m_CEESubsystem)); + // CEE testing binding + m_CEESubsystem.setDefaultCommand( + new InstantCommand( + () -> + m_CEESubsystem.setVoltage( + m_auxController.getRightTriggerAxis() * RobotStateConstants.MAX_VOLTAGE), + m_CEESubsystem)); + m_auxController + .rightBumper() + .onTrue( + Commands.run( + () -> { + m_CEESubsystem.enablePID(true); + m_CEESubsystem.setSetpoint(Units.rotationsPerMinuteToRadiansPerSecond(1000)); + }, + m_CEESubsystem)) + .onFalse( + new InstantCommand( + () -> { + m_CEESubsystem.setSetpoint(0); + m_CEESubsystem.enablePID(false); + }, + m_CEESubsystem)); - // // Funnel testing binding - // m_auxController - // .povUp() - // .onTrue( - // Commands.run( - // () -> { - // m_funnelSubsystem.enablePID(true); - // - // m_funnelSubsystem.setSetpoint(Units.rotationsPerMinuteToRadiansPerSecond(1000)); - // }, - // m_funnelSubsystem)) - // .onFalse( - // new InstantCommand( - // () -> { - // m_funnelSubsystem.setSetpoint(0); - // m_funnelSubsystem.enablePID(false); - // }, - // m_funnelSubsystem)); - // m_auxController - // .povDown() - // .onTrue(new InstantCommand(() -> m_funnelSubsystem.setVoltage(12), m_funnelSubsystem)) - // .onFalse(new InstantCommand(() -> m_funnelSubsystem.setVoltage(0), m_funnelSubsystem)); + // Funnel testing binding + m_auxController + .povUp() + .onTrue( + Commands.run( + () -> { + m_funnelSubsystem.enablePID(true); - // // ALGAE Pivot testing binding - // m_auxController - // .b() - // .onTrue( - // new InstantCommand( - // () -> m_algaePivotSubsystem.setSetpoint(AlgaePivotConstants.MAX_ANGLE_RAD), - // m_algaePivotSubsystem)) - // .onFalse( - // new InstantCommand( - // () -> m_algaePivotSubsystem.setSetpoint(AlgaePivotConstants.DEFAULT_ANGLE_RAD), - // m_algaePivotSubsystem)); - // m_auxController - // .x() - // .onTrue( - // new InstantCommand( - // () -> m_algaePivotSubsystem.setSetpoint(AlgaePivotConstants.MIN_ANGLE_RAD), - // m_algaePivotSubsystem)) - // .onFalse( - // new InstantCommand( - // () -> m_algaePivotSubsystem.setSetpoint(AlgaePivotConstants.DEFAULT_ANGLE_RAD), - // m_algaePivotSubsystem)); + m_funnelSubsystem.setSetpoint(Units.rotationsPerMinuteToRadiansPerSecond(1000)); + }, + m_funnelSubsystem)) + .onFalse( + new InstantCommand( + () -> { + m_funnelSubsystem.setSetpoint(0); + m_funnelSubsystem.enablePID(false); + }, + m_funnelSubsystem)); + m_auxController + .povDown() + .onTrue(new InstantCommand(() -> m_funnelSubsystem.setVoltage(12), m_funnelSubsystem)) + .onFalse(new InstantCommand(() -> m_funnelSubsystem.setVoltage(0), m_funnelSubsystem)); - // // Periscope testing binding - // m_auxController - // .a() - // .onTrue( - // new InstantCommand( - // () -> m_periscopeSubsystem.setPosition(PeriscopeConstants.MAX_HEIGHT_M), - // m_periscopeSubsystem)) - // .onFalse( - // new InstantCommand( - // () -> m_periscopeSubsystem.setPosition(PeriscopeConstants.MIN_HEIGHT_M), - // m_periscopeSubsystem)); + // ALGAE Pivot testing binding + m_auxController + .b() + .onTrue( + new InstantCommand( + () -> m_algaePivotSubsystem.setSetpoint(AlgaePivotConstants.MAX_ANGLE_RAD), + m_algaePivotSubsystem)) + .onFalse( + new InstantCommand( + () -> m_algaePivotSubsystem.setSetpoint(AlgaePivotConstants.DEFAULT_ANGLE_RAD), + m_algaePivotSubsystem)); + m_auxController + .x() + .onTrue( + new InstantCommand( + () -> m_algaePivotSubsystem.setSetpoint(AlgaePivotConstants.MIN_ANGLE_RAD), + m_algaePivotSubsystem)) + .onFalse( + new InstantCommand( + () -> m_algaePivotSubsystem.setSetpoint(AlgaePivotConstants.DEFAULT_ANGLE_RAD), + m_algaePivotSubsystem)); + + // Periscope testing binding + m_auxController + .a() + .onTrue( + new InstantCommand( + () -> m_periscopeSubsystem.setPosition(PeriscopeConstants.MAX_HEIGHT_M), + m_periscopeSubsystem)) + .onFalse( + new InstantCommand( + () -> m_periscopeSubsystem.setPosition(PeriscopeConstants.MIN_HEIGHT_M), + m_periscopeSubsystem)); - // // Climber testing binding - // m_auxController - // .y() - // .onTrue( - // new InstantCommand( - // () -> m_climberSubsystem.setPosition(ClimberConstants.MIN_ANGLE_RAD), - // m_climberSubsystem)) - // .onFalse( - // new InstantCommand( - // () -> m_climberSubsystem.setPosition(ClimberConstants.MAX_ANGLE_RAD), - // m_climberSubsystem)); + // Climber testing binding + m_auxController + .y() + .onTrue( + new InstantCommand( + () -> m_climberSubsystem.setPosition(ClimberConstants.MIN_ANGLE_RAD), + m_climberSubsystem)) + .onFalse( + new InstantCommand( + () -> m_climberSubsystem.setPosition(ClimberConstants.MAX_ANGLE_RAD), + m_climberSubsystem)); } /** @@ -428,14 +440,15 @@ public Command getAutonomousCommand() { /** * Sets all mechanisms to brake mode, intended for use when the robot is disabled. * - * @param enable - True to set brake mode, False to set coast mode + * @param enable {@code true} to enable brake mode, {@code false} to enable coast mode. */ public void allMechanismsBrakeMode(boolean enable) { m_driveSubsystem.enableBrakeModeAll(enable); - // m_climberSubsystem.enableBrakeMode(enable); - // m_AEESubsystem.enableBrakeMode(enable); - // m_algaePivotSubsystem.enableBrakeMode(enable); - // m_funnelSubsystem.enableBrakeMode(enable); - // m_periscopeSubsystem.enableBrakeMode(enable); + m_algaePivotSubsystem.enableBrakeMode(enable); + m_periscopeSubsystem.enableBrakeMode(enable); + m_climberSubsystem.enableBrakeMode(enable); + m_funnelSubsystem.enableBrakeMode(enable); + m_AEESubsystem.enableBrakeMode(enable); + m_CEESubsystem.enableBrakeMode(enable); } }