diff --git a/src/main/java/frc/robot/auton/commands/AutonCommands.java b/src/main/java/frc/robot/auton/commands/AutonCommands.java index 4ae091f7..5f012b35 100644 --- a/src/main/java/frc/robot/auton/commands/AutonCommands.java +++ b/src/main/java/frc/robot/auton/commands/AutonCommands.java @@ -15,6 +15,7 @@ import frc.robot.swerve.commands.DriveToCubeNode; import frc.robot.swerve.commands.SwerveCommands; import frc.robot.swerve.commands.SwerveDrive; +import frc.robot.vision.VisionConfig; import java.util.function.DoubleSupplier; public class AutonCommands { @@ -166,7 +167,7 @@ public static Command simpleLaunchCube() { } public static Command alignToGridMid() { - return new DriveToCubeNode(0) + return new DriveToCubeNode(0, VisionConfig.aprilTagPipeline) .alongWith(ElevatorCommands.cubeMid()) .withTimeout(0.75) .andThen(spinLauncherFast(IntakeCommands.midCubeSpinUp())) @@ -174,7 +175,7 @@ public static Command alignToGridMid() { } public static Command alignToGridHigh() { - return new DriveToCubeNode(0) + return new DriveToCubeNode(0, VisionConfig.aprilTagPipeline) .alongWith(ElevatorCommands.cubeTop()) .withTimeout(0.75) .andThen(spinLauncherFast(IntakeCommands.topCubeSpinUp())) @@ -182,7 +183,7 @@ public static Command alignToGridHigh() { } public static Command alignToGridMidFast() { - return new DriveToCubeNode(0) + return new DriveToCubeNode(0, VisionConfig.aprilTagPipeline) .alongWith(ElevatorCommands.cubeMid()) .withTimeout(0.45) .andThen(spinLauncherFast(IntakeCommands.midCubeSpinUp())) @@ -190,7 +191,7 @@ public static Command alignToGridMidFast() { } public static Command alignToGridHighFast() { - return new DriveToCubeNode(0) + return new DriveToCubeNode(0, VisionConfig.aprilTagPipeline) .alongWith(ElevatorCommands.cubeTop()) .withTimeout(0.45) .andThen(spinLauncherFast(IntakeCommands.topCubeSpinUp())) @@ -198,7 +199,7 @@ public static Command alignToGridHighFast() { } public static Command alignToGridLowCube() { - return new AlignToAprilTag(() -> -0.75, 0) + return new AlignToAprilTag(() -> -0.75, 0, VisionConfig.aprilTagPipeline) .withTimeout(1) .alongWith(IntakeCommands.hybridShot()) .withTimeout(1) @@ -206,7 +207,7 @@ public static Command alignToGridLowCube() { } public static Command alignToGridLowCone() { - return new AlignToAprilTag(() -> -0.75, 0) + return new AlignToAprilTag(() -> -0.75, 0, VisionConfig.aprilTagPipeline) .withTimeout(1) .alongWith(ElevatorCommands.coneFloorGoal()) .alongWith(FourBarCommands.coneFloorGoal()) diff --git a/src/main/java/frc/robot/pilot/PilotGamepad.java b/src/main/java/frc/robot/pilot/PilotGamepad.java index ecd4e50a..e24375ad 100644 --- a/src/main/java/frc/robot/pilot/PilotGamepad.java +++ b/src/main/java/frc/robot/pilot/PilotGamepad.java @@ -12,7 +12,9 @@ import frc.robot.leds.commands.OneColorLEDCommand; import frc.robot.pilot.commands.PilotCommands; import frc.robot.swerve.commands.AlignToAprilTag; +import frc.robot.swerve.commands.DriveToCubeNode; import frc.robot.trajectories.commands.DistanceDrive; +import frc.robot.vision.VisionConfig; /** Used to add buttons to the pilot gamepad and configure the joysticks */ public class PilotGamepad extends Gamepad { @@ -80,21 +82,27 @@ public void setupTeleopButtons() { gamepad.Dpad.Right.and(noBumpers()).whileTrue(new DistanceDrive(Units.inchesToMeters(-5))); /* Aligning */ - rightBumperOnly() - .whileTrue(new AlignToAprilTag(() -> Robot.pilotGamepad.getDriveFwdPositive(), 0)); - // rightBumperOnly().whileTrue(new DriveToCubeNode(0)); + // rightBumperOnly() + // .whileTrue( + // new AlignToAprilTag( + // () -> Robot.pilotGamepad.getDriveFwdPositive(), + // 0, + // VisionConfig.detectorPipeline)); + rightBumperOnly().whileTrue(new DriveToCubeNode(0, VisionConfig.detectorPipeline)); rightBumperOnly() .and(rightTrigger) .whileTrue( new AlignToAprilTag( () -> Robot.pilotGamepad.getDriveFwdPositive(), - PilotConfig.alignmentOffset)); + PilotConfig.alignmentOffset, + VisionConfig.aprilTagPipeline)); rightBumperOnly() .and(leftTrigger) .whileTrue( new AlignToAprilTag( () -> Robot.pilotGamepad.getDriveFwdPositive(), - -PilotConfig.alignmentOffset)); + -PilotConfig.alignmentOffset, + VisionConfig.aprilTagPipeline)); /* Reorient */ gamepad.Dpad.Up.and(leftBumperOnly()).whileTrue(PilotCommands.reorient(0)); diff --git a/src/main/java/frc/robot/swerve/commands/AlignToAprilTag.java b/src/main/java/frc/robot/swerve/commands/AlignToAprilTag.java index f5f35c47..26ed8025 100644 --- a/src/main/java/frc/robot/swerve/commands/AlignToAprilTag.java +++ b/src/main/java/frc/robot/swerve/commands/AlignToAprilTag.java @@ -11,6 +11,7 @@ import frc.robot.Robot; import frc.robot.leds.commands.LEDCommands; import frc.robot.leds.commands.OneColorLEDCommand; +import frc.robot.vision.VisionConfig; import java.util.function.DoubleSupplier; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -24,14 +25,21 @@ public class AlignToAprilTag extends PIDCommand { SwerveDrive driveCommand; DoubleSupplier fwdPositiveSupplier; private static double out; + private int pipelineIndex; - /** Creates a new AlignToAprilTag. */ - public AlignToAprilTag(DoubleSupplier fwdPositiveSupplier, double offset) { + /** + * Creates a new AlignToAprilTag //TODO: change naming, not just apriltag anymore + * + * @param fwdPositiveSupplier + * @param offset + * @param pipeline which limelight pipeline to set to. Apriltag, retro, detector + */ + public AlignToAprilTag(DoubleSupplier fwdPositiveSupplier, double offset, int pipelineIndex) { super( // The controller that the command will use new PIDController(lowKP, 0, 0), // This should return the measurement - () -> Robot.vision.getHorizontalOffset(), + () -> (-Robot.vision.getHorizontalOffset()), // This should return the setpoint (can also be a constant) () -> offset, // This uses the output @@ -48,11 +56,12 @@ public AlignToAprilTag(DoubleSupplier fwdPositiveSupplier, double offset) { () -> true); // Field relative is true // Use addRequirements() here to declare subsystem dependencies. // Configure additional PID options by calling `getController` here. + this.pipelineIndex = pipelineIndex; this.setName("AlignToAprilTag"); } public double getSteering() { - return Robot.swerve.calculateRotationController(() -> Math.PI); + return Robot.swerve.calculateRotationController(() -> 0); } public static void setOutput(double output) { @@ -80,6 +89,8 @@ public void initialize() { } else { this.getController().setP(lowKP); } + + Robot.vision.setLimelightPipeline(pipelineIndex); } @Override @@ -92,6 +103,8 @@ public void execute() { @Override public void end(boolean interrupted) { // getLedCommand(tagID).end(interrupted); + Robot.vision.setLimelightPipeline(VisionConfig.aprilTagPipeline); + driveCommand.end(interrupted); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/swerve/commands/DriveToCubeNode.java b/src/main/java/frc/robot/swerve/commands/DriveToCubeNode.java index fb2ab779..9c835fc8 100644 --- a/src/main/java/frc/robot/swerve/commands/DriveToCubeNode.java +++ b/src/main/java/frc/robot/swerve/commands/DriveToCubeNode.java @@ -15,7 +15,7 @@ public class DriveToCubeNode extends PIDCommand { /* Config settings */ private static double kP = 0.06; - private static double verticalSetpoint = -15; // neg + private static double verticalSetpoint = -18; // neg private double tolerance = 1; private double horizontalOffset = 0; // positive is right (driver POV) @@ -28,7 +28,7 @@ public class DriveToCubeNode extends PIDCommand { * @param horizontalOffset adjustable offset in the Y axis in case robot isn't completely * aligned. Default value should be 0 */ - public DriveToCubeNode(double horizontalOffset) { + public DriveToCubeNode(double horizontalOffset, int pipelineIndex) { super( // The controller that the command will use new PIDController(kP, 0, 0), @@ -40,7 +40,7 @@ public DriveToCubeNode(double horizontalOffset) { output -> { setOutput(output); }); - alignToTag = new AlignToAprilTag(() -> getOutput(), horizontalOffset); + alignToTag = new AlignToAprilTag(() -> (-getOutput()), horizontalOffset, pipelineIndex); // Use addRequirements() here to declare subsystem dependencies. addRequirements(Robot.swerve); // Configure additional PID options by calling `getController` here. diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index f5e1a460..62164bbe 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -384,4 +384,9 @@ private void aimingPrintDebug( System.out.println(" needed new theta: " + df.format(theta)); } } + + /** @param pipelineIndex use pipeline indexes in {@link VisionConfig} */ + public void setLimelightPipeline(int pipelineIndex) { + LimelightHelpers.setPipelineIndex(null, pipelineIndex); + } } diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index 8a1db956..6e101419 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -14,6 +14,11 @@ public final class VisionConfig { /* Limelight Configuration */ + /* Pipeline config */ + public static final int aprilTagPipeline = 0; + public static final int reflectivePipeline = 1; + public static final int detectorPipeline = 2; + /* How many degrees back is limelight rotated from perfectly vertical */ public static final double limelightAngle = -22.5; public static final double limelightLensHeight = 41.374; // inches