align to source

grt192 · Feb 25, 2025 · 23d8a5f · 23d8a5f
1 parent a5c5d2a
commit 23d8a5f
Show file tree

Hide file tree

Showing 4 changed files with 262 additions and 125 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -36,6 +36,7 @@
 import frc.robot.subsystems.Vision.CameraConfig;
 import frc.robot.util.PolynomialRegression;
 import frc.robot.util.Motors.LoggedSparkMaxConfig;
+import frc.robot.subsystems.AlignSubsystem;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -217,5 +218,132 @@ public static class IntakeConstants {
     )
 	);
 
+  public static class SourceAlignConstants {
+
+    static AlignSubsystem alignSubsystem = new AlignSubsystem();
+    public static String LS_1alignName  = "LS align 1";
+    public static String LS_2alignName  = "LS align 2";
+    public static String LS_3alignName  = "LS align 3";
+    public static String RS_1alignName  = "RS align 1";
+    public static String RS_2alignName  = "RS align 2";
+    public static String RS_3alignName  = "RS align 3";
+
+
+    public static final List<Pose2d> blueSourcePoses = List.of(
+      alignSubsystem.getAlignPath(LS_1alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(LS_2alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(LS_3alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(RS_1alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(RS_2alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(RS_3alignName).getStartingHolonomicPose().get()
+    );
+
+    public static final List<Pose2d> redSourcePoses = List.of(
+      alignSubsystem.getAlignPath(LS_1alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(LS_2alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(LS_3alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(RS_1alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(RS_2alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(RS_3alignName).flipPath().getStartingHolonomicPose().get()
+    );
+
+    public static final List<String> sourcePathList = List.of(
+      LS_1alignName,
+      LS_2alignName,
+      LS_3alignName,
+      RS_1alignName,
+      RS_2alignName,
+      RS_3alignName
+    );
   }
+
+  public static class ReefAlignConstants {
+
+    static AlignSubsystem alignSubsystem = new AlignSubsystem();
+    public static String reefName = "reefAlignPath";
+    public static String sourceName = "sourceAlignPath";
+    public static String A_alignName = "A align";
+    public static String B_alignName = "B align";
+    public static String C_alignName = "C align";
+    public static String D_alignName = "D align";
+    public static String E_alignName = "E align";
+    public static String F_alignName = "F align";
+    public static String G_alignName = "G align";
+    public static String H_alignName = "H align";
+    public static String I_alignName = "I align";
+    public static String J_alignName = "J align";
+    public static String K_alignName = "K align";
+    public static String L_alignName = "L align";
+
+
+
+    public final static List<Pose2d> blueLeftReefPoseList = List.of(
+        alignSubsystem.getAlignPath(A_alignName).getStartingHolonomicPose().get(),
+        alignSubsystem.getAlignPath(C_alignName).getStartingHolonomicPose().get(),
+        alignSubsystem.getAlignPath(E_alignName).getStartingHolonomicPose().get(),
+        alignSubsystem.getAlignPath(G_alignName).getStartingHolonomicPose().get(),
+        alignSubsystem.getAlignPath(I_alignName).getStartingHolonomicPose().get(),
+        alignSubsystem.getAlignPath(K_alignName).getStartingHolonomicPose().get()
+    );
+
+    public final static List<Pose2d> blueRightReefPoseList = List.of(
+      alignSubsystem.getAlignPath(B_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(D_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(F_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(H_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(J_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(L_alignName).getStartingHolonomicPose().get()
+    ); 
+
+
+    public final static List<Pose2d> redLeftReefPoseList = List.of(
+      alignSubsystem.getAlignPath(A_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(C_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(E_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(G_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(I_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(K_alignName).flipPath().getStartingHolonomicPose().get()
+    );
+
+    public final static List<Pose2d> redRightReefPoseList = List.of(
+      alignSubsystem.getAlignPath(B_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(D_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(F_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(H_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(J_alignName).flipPath().getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(L_alignName).flipPath().getStartingHolonomicPose().get()
+    ); 
+
+    //put the left on top of right
+    public final static List<Pose2d> allReefPoseList = List.of(
+      alignSubsystem.getAlignPath(B_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem. getAlignPath(C_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem. getAlignPath(D_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem. getAlignPath(E_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem. getAlignPath(F_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem. getAlignPath(G_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem. getAlignPath(H_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(I_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(J_alignName).getStartingHolonomicPose().get(),
+      alignSubsystem.getAlignPath(K_alignName).getStartingHolonomicPose().get(),
+       alignSubsystem.getAlignPath(L_alignName).getStartingHolonomicPose().get()
+    );
+
+    //put the left on top of right
+    public final static List<String> reefPathList = List.of(
+        A_alignName,
+        B_alignName,
+        C_alignName,
+        D_alignName,
+        E_alignName,
+        F_alignName,
+        G_alignName,
+        H_alignName,
+        I_alignName,
+        J_alignName,
+        K_alignName,
+        L_alignName
+    );
+  }
+}
 }
diff --git a/src/main/java/frc/robot/commands/LRReefAlignCommand.java b/src/main/java/frc/robot/commands/LRReefAlignCommand.java
@@ -10,104 +10,24 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.AlignSubsystem;
 import frc.robot.subsystems.FieldManagementSubsystem.FieldManagementSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.Constants.IntakeConstants.ReefAlignConstants;
+import frc.robot.subsystems.AlignSubsystem;
 
 public class LRReefAlignCommand extends Command{
     static SwerveSubsystem swerveSubsystem;
     static FieldManagementSubsystem fmsSubsystem;
+    static AlignSubsystem alignSubsystem;
     static List<Pose2d> currentRightPoseList;
     static List<Pose2d> currentLeftPoseList;
     Boolean isRight;
     static PathPlannerPath getAlignPath;
 
     private static Command runAlignPath;
 
-    private static String reefName = "reefAlignPath";
-    private static String sourceName = "sourceAlignPath";
-    private static String A_alignName = "A align";
-    private static String B_alignName = "B align";
-    private static String C_alignName = "C align";
-    private static String D_alignName = "D align";
-    private static String E_alignName = "E align";
-    private static String F_alignName = "F align";
-    private static String G_alignName = "G align";
-    private static String H_alignName = "H align";
-    private static String I_alignName = "I align";
-    private static String J_alignName = "J align";
-    private static String K_alignName = "K align";
-    private static String L_alignName = "L align";
     private static String followPath;
-
-
-    static List<Pose2d> blueLeftReefPoseList = List.of(
-        getAlignPath(A_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(C_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(E_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(G_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(I_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(K_alignName).getStartingHolonomicPose().get()
-    );
-
-    static List<Pose2d> blueRightReefPoseList = List.of(
-        getAlignPath(B_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(D_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(F_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(H_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(J_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(L_alignName).getStartingHolonomicPose().get()
-    ); 
-
-
-    static List<Pose2d> redLeftReefPoseList = List.of(
-        getAlignPath(A_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(C_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(E_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(G_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(I_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(K_alignName).flipPath().getStartingHolonomicPose().get()
-    );
-
-    static List<Pose2d> redRightReefPoseList = List.of(
-        getAlignPath(B_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(D_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(F_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(H_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(J_alignName).flipPath().getStartingHolonomicPose().get(),
-        getAlignPath(L_alignName).flipPath().getStartingHolonomicPose().get()
-    ); 
-
-    //put the left on top of right
-    static List<Pose2d> allReefPoseList = List.of(
-        getAlignPath(A_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(B_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(C_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(D_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(E_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(F_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(G_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(H_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(I_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(J_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(K_alignName).getStartingHolonomicPose().get(),
-        getAlignPath(L_alignName).getStartingHolonomicPose().get()
-    );
-
-    //put the left on top of right
-    static List<String> reefPathList = List.of(
-        A_alignName,
-        B_alignName,
-        C_alignName,
-        D_alignName,
-        E_alignName,
-        F_alignName,
-        G_alignName,
-        H_alignName,
-        I_alignName,
-        J_alignName,
-        K_alignName,
-        L_alignName
-    );
 
     static PathConstraints constraints = new PathConstraints(
         4.6,
@@ -116,73 +36,41 @@ public class LRReefAlignCommand extends Command{
         Units.degreesToRadians(720)
     );
 
-    public LRReefAlignCommand(SwerveSubsystem swerveSubsystem, FieldManagementSubsystem fmsSubsystem, Boolean isRight) {
+    public LRReefAlignCommand(SwerveSubsystem swerveSubsystem, FieldManagementSubsystem fmsSubsystem, AlignSubsystem alignSubsystem, Boolean isRight) {
         this.swerveSubsystem = swerveSubsystem;
         this.fmsSubsystem = fmsSubsystem;
+        this.alignSubsystem = alignSubsystem;
         this.isRight = isRight;
     }
 
     @Override
     public void initialize() {
         if (fmsSubsystem.isRedAlliance()) {
-            currentRightPoseList = redRightReefPoseList;
-            currentLeftPoseList = redLeftReefPoseList;
+            currentRightPoseList = ReefAlignConstants.redRightReefPoseList;
+            currentLeftPoseList = ReefAlignConstants.redLeftReefPoseList;
         }
         else {
-            currentRightPoseList = blueRightReefPoseList;
-            currentLeftPoseList = blueLeftReefPoseList;
+            currentRightPoseList = ReefAlignConstants.blueRightReefPoseList;
+            currentLeftPoseList = ReefAlignConstants.blueLeftReefPoseList;
         }
         if (isRight){
             Pose2d closestRight = swerveSubsystem.getRobotPosition().nearest(currentRightPoseList);
             int index = currentRightPoseList.indexOf(closestRight);
-            followPath = reefPathList.get(2 * index + 1);
+            followPath = ReefAlignConstants.reefPathList.get(2 * index + 1);
         }
         else {
             Pose2d closestLeft = swerveSubsystem.getRobotPosition().nearest(currentLeftPoseList);
             // System.out.println(swerveSubsystem.getRobotPosition());
             int index = currentLeftPoseList.indexOf(closestLeft);
-            followPath = reefPathList.get(2 * index);
+            followPath = ReefAlignConstants.reefPathList.get(2 * index);
         }
         System.out.println("InITing");
-        runAlignPath(followPath).schedule();
+        alignSubsystem.runAlignPath(followPath).schedule();
     }
 
     @Override
     public void end(boolean interrupted) {
         swerveSubsystem.getCurrentCommand().cancel();
     }
 
-        /**
-     * Uses getAlignPath to get the pathplanner path and follows it
-     * @param swerveSubsystem
-     * @param pathName name of the path
-     */
-    public static Command runAlignPath (String pathName) {
-        PathPlannerPath path = getAlignPath(pathName);
-        // System.out.print(path);
-
-        runAlignPath = AutoBuilder.pathfindThenFollowPath(
-            path,
-            constraints
-        );
-
-        runAlignPath.addRequirements(swerveSubsystem); 
-        System.out.println("RUNNING");
-        return runAlignPath;
-    }
-
-        /**
-     * takes the path name and returns the PathPlanner Path 
-     * @param pathName
-     * @return path file
-     */
-    private static PathPlannerPath getAlignPath(String pathName) {
-        try {
-            getAlignPath = PathPlannerPath.fromPathFile(pathName);
-        } catch (Exception e) {
-            e.printStackTrace();
-            // Handle exception as needed, maybe use default values or fallback
-        }
-        return getAlignPath;
-    }
 }
diff --git a/src/main/java/frc/robot/commands/SourceAlignCommand.java b/src/main/java/frc/robot/commands/SourceAlignCommand.java
@@ -0,0 +1,54 @@
+package frc.robot.commands;
+
+import java.util.List;
+
+import org.opencv.photo.AlignExposures;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.FieldManagementSubsystem.FieldManagementSubsystem;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.subsystems.AlignSubsystem;
+import frc.robot.Constants.IntakeConstants.SourceAlignConstants;
+
+
+public class SourceAlignCommand extends Command {
+    private final SwerveSubsystem swerveSubsystem;
+    private final FieldManagementSubsystem fmsSubsystem;
+    private final AlignSubsystem alignSubsystem;
+    static String followPath;
+    static List<Pose2d> currentPoseList;
+    static Pose2d currentPose;
+
+
+    public SourceAlignCommand(AlignSubsystem alignSubsystem, SwerveSubsystem swerveSubsystem, FieldManagementSubsystem fmsSubsystem){
+        this.alignSubsystem = alignSubsystem;
+        this.swerveSubsystem = swerveSubsystem;
+        this.fmsSubsystem = fmsSubsystem;
+    }
+
+   @Override
+   public void initialize() {
+    if (fmsSubsystem.isRedAlliance()){
+        currentPoseList = SourceAlignConstants.redSourcePoses;
+    }
+    else {
+        currentPoseList = SourceAlignConstants.blueSourcePoses;
+    }
+    currentPose = swerveSubsystem.getRobotPosition().nearest(currentPoseList);
+    int index = currentPoseList.indexOf(currentPose);
+    followPath = SourceAlignConstants.sourcePathList.get(index);
+
+    alignSubsystem.runAlignPath(followPath).schedule();
+   }
+
+   @Override
+   public void end(boolean interrupted) {
+       swerveSubsystem.getCurrentCommand().cancel();
+   }
+
+}