Merge pull request #35 from TotalTaxAmount/reef-align

Reef align

Author: aaditsangvikar

src/main/java/frc/robot/Robot.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import org.littletonrobotics.junction.Logger;
 
 public class Robot extends TimedRobot {
     private Command autonomousCommand;

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

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.commands.*;
 import frc.robot.constants.*;
+import frc.robot.framework.Odometry;
 import frc.robot.subsystems.*;
 import frc.robot.utils.Camera;
 import frc.robot.utils.Controller;
@@ -28,7 +29,7 @@ public class RobotContainer {
     private final Camera testCamera =
             new Camera("testCam", Camera.CameraType.PHOTONVISION, cameraOffset);
 
-    private OdometrySubsystem odometrySubsystem = OdometrySubsystem.getInstance();
+    private Odometry odometry = Odometry.getInstance();
     // Auto Chooser
     SendableChooser<Command> superSecretMissileTech = new SendableChooser<>();
 
@@ -57,11 +58,11 @@ private void configureBindings() {
     }
 
     public void robotInit() {
-        odometrySubsystem.addCamera(testCamera);
+        odometry.addCamera(testCamera);
     }
 
     public void robotPeriodic() {
-        odometrySubsystem.periodic();
+        odometry.periodic();
     }
 
     public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/ReefAlignCommand.java

@@ -0,0 +1,82 @@
+/* Black Knights Robotics (C) 2025 */
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.AlignConstants;
+import frc.robot.framework.Odometry;
+import frc.robot.subsystems.SwerveSubsystem;
+import frc.robot.utils.ConfigManager;
+
+public class ReefAlignCommand extends Command {
+    private final SwerveSubsystem swerveSubsystem;
+
+    private ProfiledPIDController xAxisPid =
+            new ProfiledPIDController(
+                    AlignConstants.X_AXIS_P,
+                    AlignConstants.X_AXIS_I,
+                    AlignConstants.X_AXIS_D,
+                    AlignConstants.X_AXIS_CONSTRAINTS);
+
+    private ProfiledPIDController yAxisPid =
+            new ProfiledPIDController(
+                    AlignConstants.Y_AXIS_P,
+                    AlignConstants.Y_AXIS_I,
+                    AlignConstants.Y_AXIS_D,
+                    AlignConstants.Y_AXIS_CONSTRAINTS);
+
+    private ProfiledPIDController rotationPid =
+            new ProfiledPIDController(
+                    AlignConstants.ROTATION_P,
+                    AlignConstants.ROTATION_I,
+                    AlignConstants.ROTATION_D,
+                    AlignConstants.ROTATION_CONSTRAINTS);
+
+    private final Odometry odometry = Odometry.getInstance();
+    private final ConfigManager configManager = ConfigManager.getInstance();
+
+    private Pose3d targetPos =
+            new Pose3d(497.77, 130.17, 0.0, new Rotation3d(0, 0, Math.toRadians(270)));
+
+    public ReefAlignCommand(SwerveSubsystem swerveSubsystem) {
+        this.swerveSubsystem = swerveSubsystem;
+
+        this.xAxisPid.setTolerance(configManager.get("reef_align_pos_tolerance", 0.2));
+        this.yAxisPid.setTolerance(configManager.get("reef_align_pos_tolerance", 0.2));
+        this.rotationPid.setTolerance(
+                Math.toRadians(configManager.get("reef_align_rotation_tolerance", 10)));
+
+        addRequirements(swerveSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        Pose3d robotPose = odometry.getRobotPose();
+        double xAxisCalc = this.xAxisPid.calculate(robotPose.getX());
+        double yAxisCalc = this.yAxisPid.calculate(robotPose.getY());
+        double rotationPidCalc = this.rotationPid.calculate(robotPose.getRotation().getZ());
+
+        swerveSubsystem.drive(xAxisCalc, yAxisCalc, rotationPidCalc, true, true);
+    }
+
+    @Override
+    public void initialize() {
+        // Reset PIDs to starting pose
+        Pose3d robotPose = odometry.getRobotPose();
+        this.xAxisPid.reset(robotPose.getX());
+        this.yAxisPid.reset(robotPose.getY());
+        this.rotationPid.reset(robotPose.getRotation().getZ());
+
+        // Set PIDs to target
+        this.xAxisPid.setGoal(targetPos.getX());
+        this.yAxisPid.setGoal(targetPos.getY());
+        this.rotationPid.setGoal(targetPos.getRotation().getZ());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return xAxisPid.atGoal() && yAxisPid.atGoal() && rotationPid.atGoal();
+    }
+}

src/main/java/frc/robot/constants/AlignConstants.java

@@ -0,0 +1,24 @@
+/* Black Knights Robotics (C) 2025 */
+package frc.robot.constants;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+public class AlignConstants {
+    public static final double X_AXIS_P = 0.0;
+    public static final double X_AXIS_I = 0.0;
+    public static final double X_AXIS_D = 0.0;
+    public static final TrapezoidProfile.Constraints X_AXIS_CONSTRAINTS =
+            new TrapezoidProfile.Constraints(3, 3);
+
+    public static final double Y_AXIS_P = 0.0;
+    public static final double Y_AXIS_I = 0.0;
+    public static final double Y_AXIS_D = 0.0;
+    public static final TrapezoidProfile.Constraints Y_AXIS_CONSTRAINTS =
+            new TrapezoidProfile.Constraints(3, 3);
+
+    public static final double ROTATION_P = 0.0;
+    public static final double ROTATION_I = 0.0;
+    public static final double ROTATION_D = 0.0;
+    public static final TrapezoidProfile.Constraints ROTATION_CONSTRAINTS =
+            new TrapezoidProfile.Constraints(Math.PI, Math.PI);
+}

src/main/java/frc/robot/subsystems/OdometrySubsystem.java renamed to src/main/java/frc/robot/framework/Odometry.java

@@ -1,5 +1,5 @@
 /* Black Knights Robotics (C) 2025 */
-package frc.robot.subsystems;
+package frc.robot.framework;
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -17,12 +17,12 @@
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
-public class OdometrySubsystem {
+public class Odometry {
     /** List of cameras used for vision-based measurements to refine odometry. */
     private ArrayList<Camera> cameras = new ArrayList<>();
 
     /** Singleton instance of the OdometrySubsystem. */
-    private static OdometrySubsystem INSTANCE = null;
+    private static Odometry INSTANCE = null;
 
     /** Logger for recording debug and error messages related to odometry subsystem operations. */
     private static final Logger LOGGER = LogManager.getLogger();
@@ -66,22 +66,22 @@ public class OdometrySubsystem {
                                 1
                             }));
 
-    private OdometrySubsystem() {}
+    private Odometry() {}
 
     /**
-     * Get the instance of OdometrySubsystem, creating a new one if it doesn't exist
+     * Get the instance of Odometry, creating a new one if it doesn't exist
      *
      * @return The instance
      */
-    public static OdometrySubsystem getInstance() {
+    public static Odometry getInstance() {
         if (INSTANCE == null) {
-            INSTANCE = new OdometrySubsystem();
+            INSTANCE = new Odometry();
         }
         return INSTANCE;
     }
 
     /**
-     * Add a camera to the odometry subsystem for vision-based localization.
+     * Add a camera to the Odometry for vision-based localization.
      *
      * @param camera The {@link Camera} to add to the system.
      */
@@ -107,13 +107,14 @@ public Pose3d getRobotPose() {
     public void addWheelOdometry(
             Rotation3d gyroRotation, SwerveModulePosition[] swerveModulePositions) {
         if (swerveModulePositions.length != 4) {
-            // LOGGER.error("Wrong length for module positions");
+            LOGGER.error("Wrong length for module positions");
             return;
         }
 
         this.poseEstimator.update(gyroRotation, swerveModulePositions);
     }
 
+    @Override
     public void periodic() {
         NTTelemetry.setArrayEntry(
                 "Pose",