DO NOT USE THIS

PLEASE
Spectrum3847 · Aug 21, 2023 · ff3af60 · ff3af60
1 parent e3bf1a7
commit ff3af60
Show file tree

Hide file tree

Showing 6 changed files with 50 additions and 18 deletions.
diff --git 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,47 +167,47 @@ 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()))
                 .andThen(launch(), stopMotors());
     }
 
     public static Command alignToGridHigh() {
-        return new DriveToCubeNode(0)
+        return new DriveToCubeNode(0, VisionConfig.aprilTagPipeline)
                 .alongWith(ElevatorCommands.cubeTop())
                 .withTimeout(0.75)
                 .andThen(spinLauncherFast(IntakeCommands.topCubeSpinUp()))
                 .andThen(launch(), stopMotors());
     }
 
     public static Command alignToGridMidFast() {
-        return new DriveToCubeNode(0)
+        return new DriveToCubeNode(0, VisionConfig.aprilTagPipeline)
                 .alongWith(ElevatorCommands.cubeMid())
                 .withTimeout(0.45)
                 .andThen(spinLauncherFast(IntakeCommands.midCubeSpinUp()))
                 .andThen(launch(), stopMotors());
     }
 
     public static Command alignToGridHighFast() {
-        return new DriveToCubeNode(0)
+        return new DriveToCubeNode(0, VisionConfig.aprilTagPipeline)
                 .alongWith(ElevatorCommands.cubeTop())
                 .withTimeout(0.45)
                 .andThen(spinLauncherFast(IntakeCommands.topCubeSpinUp()))
                 .andThen(launch(), stopMotors());
     }
 
     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)
                 .andThen(AutonCommands.launch(), AutonCommands.stopMotors());
     }
 
     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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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