Climb DA

src/main/java/frc/robot/Constants.java

@@ -7,6 +7,10 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.ArrayList;
+
 public final class Constants {
   // We do not include the AdvKit in the main FeatureFlags class - since this is
   // in Robot.java and we'd prefer not to
@@ -56,7 +60,7 @@ public static final class FeatureFlags {
     public static final boolean kRobotVizEnabled = true && !Robot.isReal();
 
     // features
-    public static final boolean kAutoAlignEnabled = false;
+    public static final boolean kAutoAlignEnabled = true;
     public static final boolean kIntakeAutoScoreDistanceSensorOffset = false;
     public static final boolean kShuffleboardLayoutEnabled = true;
     public static final boolean kUsePrefs = true;
@@ -75,4 +79,37 @@ public static final class ShuffleboardConstants {
     public static final String kArmLayoutName = "Arm";
     public static final String kShooterLayoutName = "Shooter";
   }
+
+  public static final class FieldConstants {
+    public static final ArrayList<Pose2d> kStagePosesBlue =
+        new ArrayList<>() {
+          {
+            // Podium
+            add(new Pose2d(2.657515287399292, 4.105274677276611, new Rotation2d(0)));
+            // Amp side
+            add(
+                new Pose2d(
+                    5.990447521209717, 6.171302795410156, new Rotation2d(-2.015216124571914)));
+            // Etc
+            add(
+                new Pose2d(
+                    6.10739278793335, 2.1172094345092773, new Rotation2d(2.118189174278151)));
+          }
+        };
+    public static final ArrayList<Pose2d> kStagePosesRed =
+        new ArrayList<>() {
+          {
+            // Podium
+            add(new Pose2d(13.981689453125, 4.105274677276611, new Rotation2d(3.1415926536)));
+            // Amp side
+            add(
+                new Pose2d(
+                    10.512321472167969, 6.11283016204834, new Rotation2d(-1.0445000982232164)));
+            // Etc
+            add(
+                new Pose2d(
+                    10.492830276489258, 2.0977187156677246, new Rotation2d(1.0214219124306612)));
+          }
+        };
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -308,18 +308,26 @@ public RobotContainer() {
 
     // operator.povLeft().onTrue(cancelCommand);
     // Configure the auto
-    //    if (FeatureFlags.kSwerveEnabled) {
-    //      autoChooser = AutoBuilder.buildAutoChooser();
-    //    } else {
-    //      autoChooser = new SendableChooser<>();
-    //    }
+    // if (FeatureFlags.kSwerveEnabled) {
+    // autoChooser = AutoBuilder.buildAutoChooser();
+    // } else {
+    // autoChooser = new SendableChooser<>();
+    // }
     autoChooser = new SendableChooser<>();
     autoChooser.setDefaultOption("Do Nothing", new InstantCommand());
     autoChooser.addOption(
         "5 Note test",
         AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake));
     // Autos
     SmartDashboard.putData("Auto Chooser", autoChooser);
+
+    configureAutoAlign();
+  }
+
+  private void configureAutoAlign() {
+    if (Constants.FeatureFlags.kSwerveEnabled && Constants.FeatureFlags.kAutoAlignEnabled) {
+      // operator.a().onTrue(drivetrain.autoAlignToNearestStage());
+    }
   }
 
   private void configurePivotShooter() {
@@ -368,7 +376,7 @@ private void configureIntake() {
         .whileTrue(
             intake.setVoltage(
                 IntakeConstants.kIntakeIntakeVoltage, -IntakeConstants.kPassthroughIntakeVoltage));
-    //    driver.rightTrigger().whileTrue(intake.intakeIn());
+    // driver.rightTrigger().whileTrue(intake.intakeIn());
 
     // operator.povDown().onTrue(new IntakeOff(intake));
   }
@@ -425,8 +433,10 @@ public void configureSwerve() {
                         -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left)
                     .withRotationalRate(-driver.getRightX() * MaxAngularRate)));
 
-    /* Right stick absolute angle mode on trigger hold,
-    robot adjusts heading to the angle right joystick creates */
+    /*
+     * Right stick absolute angle mode on trigger hold,
+     * robot adjusts heading to the angle right joystick creates
+     */
     driver
         .rightTrigger()
         .whileTrue(
@@ -456,7 +466,8 @@ public void configureSwerve() {
     // Reset robot heading on button press
     driver.y().onTrue(drivetrain.runOnce(drivetrain::seedFieldRelative));
 
-    // Azimuth angle bindings. isRed == true for red alliance presets. isRed != true for blue.
+    // Azimuth angle bindings. isRed == true for red alliance presets. isRed != true
+    // for blue.
     if (isRed == true) {
       driver
           .rightBumper()

src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java

@@ -28,11 +28,13 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants;
 import frc.robot.limelight.LimelightHelpers;
 import frc.robot.subsystems.swerve.kit.KitSwerveDrivetrain;
 import frc.robot.subsystems.swerve.kit.KitSwerveRequest;
@@ -281,6 +283,24 @@ public Command pathfindToNote(Vision vision) {
     //        vision.getNotePose(this.getState().Pose), constraints, 1, 0.0);});
   }
 
+  public Command autoAlignToNearestStage() {
+    return this.defer(
+        () -> {
+          System.out.println("Running daToNearestStageAprilTag");
+          Pose2d botPose2d = this.getState().Pose;
+          return AutoBuilder.pathfindToPose(
+              botPose2d.nearest(
+                  DriverStation.getAlliance().equals(Alliance.Red)
+                      ? Constants.FieldConstants.kStagePosesRed
+                      : Constants.FieldConstants.kStagePosesBlue),
+              null,
+              0.0, // Goal end velocity in meters/sec
+              0.0 // Rotation delay distance in meters. This is how far the robot should travel
+              // before attempting to rotate.
+              );
+        });
+  }
+
   public Command rotationTest() {
     PathConstraints constraints =
         new PathConstraints(