Merge pull request #28 from NandaGuntupalli/main

Created the AutoClimber Commands (With help from Ada, and Isaac made the subsystem)

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

@@ -18,6 +18,7 @@
 import frc.robot.util.LinearInterpolation;
 import frc.robot.util.PolynomialRegression;
 import java.awt.geom.Point2D;
+import java.util.List;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -66,6 +67,66 @@ public static final class FieldConstants {
     public static Pose2d driverStationRedAlliance = new Pose2d();
   }
 
+  public static final class StationCoordinateConstants {
+
+    public static final class CenterChainPoses {
+      public static final Pose2d bottomRightChainRedStage =
+          new Pose2d(new Translation2d(12.41, 2.87), new Rotation2d(-59.30));
+      public static final Pose2d topRightChainRedStage =
+          new Pose2d(new Translation2d(12.39, 5.32), new Rotation2d(60));
+      public static final Pose2d leftChainRedStage =
+          new Pose2d(new Translation2d(10.23, 4.06), new Rotation2d(-180));
+      public static final Pose2d rightChainBlueStage =
+          new Pose2d(new Translation2d(6.32, 3.99), new Rotation2d(-2.01));
+      public static final Pose2d topLeftChainBlueStage =
+          new Pose2d(new Translation2d(4.14, 5.29), new Rotation2d(120));
+      public static final Pose2d bottomLeftChainBlueStage =
+          new Pose2d(new Translation2d(4.06, 2.89), new Rotation2d(-123.52));
+    }
+
+    public static final class LeftChainPoses {
+      public static final Pose2d bottomRightChainRedStage =
+          new Pose2d(new Translation2d(11.89, 2.59), new Rotation2d(-59.30));
+      public static final Pose2d topRightChainRedStage =
+          new Pose2d(new Translation2d(13.07, 5), new Rotation2d(60));
+      public static final Pose2d leftChainRedStage =
+          new Pose2d(new Translation2d(10.27, 4.69), new Rotation2d(-180));
+      public static final Pose2d rightChainBlueStage =
+          new Pose2d(new Translation2d(6.31, 3.57), new Rotation2d(-2.01));
+      public static final Pose2d topLeftChainBlueStage =
+          new Pose2d(new Translation2d(4.72, 5.70), new Rotation2d(120));
+      public static final Pose2d bottomLeftChainBlueStage =
+          new Pose2d(new Translation2d(3.61, 3.16), new Rotation2d(-123.52));
+    }
+
+    public static final class RightChainPoses {
+      public static final Pose2d bottomRightChainRedStage =
+          new Pose2d(new Translation2d(12.94, 3.18), new Rotation2d(-59.30));
+      public static final Pose2d topRightChainRedStage =
+          new Pose2d(new Translation2d(11.98, 5.6), new Rotation2d(60));
+      public static final Pose2d leftChainRedStage =
+          new Pose2d(new Translation2d(10.27, 3.41), new Rotation2d(-180));
+      public static final Pose2d rightChainBlueStage =
+          new Pose2d(new Translation2d(6.31, 4.63), new Rotation2d(-2.01));
+      public static final Pose2d topLeftChainBlueStage =
+          new Pose2d(new Translation2d(3.60, 5.05), new Rotation2d(120));
+      public static final Pose2d bottomLeftChainBlueStage =
+          new Pose2d(new Translation2d(4.69, 2.60), new Rotation2d(-123.52));
+    }
+
+    public static final Pose2d placeHolderPose =
+        new Pose2d(new Translation2d(353, 353), new Rotation2d(353));
+
+    public static final List<Pose2d> chainCoordinates =
+        List.of(
+            CenterChainPoses.bottomRightChainRedStage,
+            CenterChainPoses.topRightChainRedStage,
+            CenterChainPoses.leftChainRedStage,
+            CenterChainPoses.rightChainBlueStage,
+            CenterChainPoses.topLeftChainBlueStage,
+            CenterChainPoses.bottomLeftChainBlueStage);
+  }
+
   public static final class VisionConstants {
     public static final String limelightName = "limelight";
     public static final String arducamName = "Arducam_OV9281";

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

@@ -67,6 +67,12 @@ public class RobotContainer implements Logged {
   private Climber climber = new Climber();
   private LEDs leds = new LEDs();
 
+  private PathConstraints pathConstraints = 
+  new PathConstraints(SwerveConstants.maxTranslationalSpeed, 
+  SwerveConstants.maxTranslationalAcceleration, 
+  SwerveConstants.maxAngularSpeed, 
+  SwerveConstants.maxAngularAcceleration);
+
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final VirtualXboxController driverController =
       new VirtualXboxController(OperatorConstants.driverControllerPort);
@@ -259,6 +265,33 @@ private void configureDriveBindings() {
                 SwerveConstants.turboMaxTranslationalSpeed,
                 SwerveConstants.maxAngularSpeed,
                 swerve));
+
+    driverController
+        .x()
+        .whileTrue(
+            new ProxyCommand(
+                () ->
+                    AutoBuilder.pathfindToPose(
+                        swerve.getLeftChainPose(),
+                        pathConstraints)));
+
+    driverController
+        .y()
+        .whileTrue(
+            new ProxyCommand(
+                () ->
+                    AutoBuilder.pathfindToPose(
+                        swerve.getCenterChainPose(),
+                        pathConstraints)));
+
+    driverController
+        .b()
+        .whileTrue(
+            new ProxyCommand(
+                () ->
+                    AutoBuilder.pathfindToPose(
+                        swerve.getRightChainPose(),
+                        pathConstraints)));
   }
 
   private void configureIntakeBindings() {

src/main/java/frc/robot/subsystems/Swerve.java

@@ -49,6 +49,10 @@
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.FrontLeftModule;
 import frc.robot.Constants.FrontRightModule;
+import frc.robot.Constants.StationCoordinateConstants;
+import frc.robot.Constants.StationCoordinateConstants.CenterChainPoses;
+import frc.robot.Constants.StationCoordinateConstants.LeftChainPoses;
+import frc.robot.Constants.StationCoordinateConstants.RightChainPoses;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.Constants.VisionConstants.ArducamConstants;
@@ -615,6 +619,100 @@ private void updatePhotonVisionPoses() {
     }
   }
 
+  public Pose2d getNearestChain() {
+    return getPose().nearest(StationCoordinateConstants.chainCoordinates);
+  }
+
+  public Pose2d getCenterChainPose() {
+    Pose2d nearestChain = getNearestChain();
+
+    if (nearestChain == CenterChainPoses.bottomRightChainRedStage) {
+      return nearestChain;
+    }
+
+    if (nearestChain == CenterChainPoses.topRightChainRedStage) {
+      return nearestChain;
+    }
+
+    if (nearestChain == CenterChainPoses.leftChainRedStage) {
+      return nearestChain;
+    }
+
+    if (nearestChain == CenterChainPoses.rightChainBlueStage) {
+      return nearestChain;
+    }
+
+    if (nearestChain == CenterChainPoses.topLeftChainBlueStage) {
+      return nearestChain;
+    }
+
+    if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) {
+      return nearestChain;
+    } else {
+      return StationCoordinateConstants.placeHolderPose;
+    }
+  }
+
+  public Pose2d getRightChainPose() {
+    Pose2d nearestChain = getNearestChain();
+
+    if (nearestChain == CenterChainPoses.bottomRightChainRedStage) {
+      return RightChainPoses.bottomRightChainRedStage;
+    }
+
+    if (nearestChain == CenterChainPoses.topRightChainRedStage) {
+      return RightChainPoses.topRightChainRedStage;
+    }
+
+    if (nearestChain == CenterChainPoses.leftChainRedStage) {
+      return RightChainPoses.leftChainRedStage;
+    }
+
+    if (nearestChain == CenterChainPoses.rightChainBlueStage) {
+      return RightChainPoses.rightChainBlueStage;
+    }
+
+    if (nearestChain == CenterChainPoses.topLeftChainBlueStage) {
+      return RightChainPoses.topLeftChainBlueStage;
+    }
+
+    if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) {
+      return RightChainPoses.bottomLeftChainBlueStage;
+    } else {
+      return StationCoordinateConstants.placeHolderPose;
+    }
+  }
+
+  public Pose2d getLeftChainPose() {
+    Pose2d nearestChain = getNearestChain();
+
+    if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) {
+      return LeftChainPoses.bottomRightChainRedStage;
+    }
+
+    if (nearestChain == CenterChainPoses.topRightChainRedStage) {
+      return LeftChainPoses.topRightChainRedStage;
+    }
+
+    if (nearestChain == CenterChainPoses.leftChainRedStage) {
+      return LeftChainPoses.leftChainRedStage;
+    }
+
+    if (nearestChain == CenterChainPoses.rightChainBlueStage) {
+      return LeftChainPoses.rightChainBlueStage;
+    }
+
+    if (nearestChain == CenterChainPoses.topLeftChainBlueStage) {
+      return LeftChainPoses.topLeftChainBlueStage;
+    }
+
+    if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) {
+      return LeftChainPoses.bottomLeftChainBlueStage;
+    } else {
+      return StationCoordinateConstants.placeHolderPose;
+    }
+  }
+
   public void updateVisionPoseEstimates() {
     detectedTargets.clear();
     updateLimelightPoses(VisionConstants.limelightName);