Merge pull request #24 from Team4388/Debug-Autos

Debug autos

src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.5546611575005915,
+        "x": 5.96969696969697,
-        "y": 2.13657272729177
+        "y": 1.128219696969696
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 2.6833333333333327
       },
       "prevControl": {
-        "x": 5.563570773660507,
+        "x": 5.75625,
-        "y": 2.6306173444056773
+        "y": 2.256439393939394
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -62.35402463626126
+    "rotation": -60.52411099675423
   },
   "reversed": false,
   "folder": null,

src/main/java/frc4388/robot/Constants.java

@@ -105,8 +105,8 @@ public final class Constants {
         public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
 
         public static final boolean DRIFT_CORRECTION_ENABLED = true;
-        public static final boolean INVERT_X = true;
+        public static final boolean INVERT_X = false;
-        public static final boolean INVERT_Y = false;
+        public static final boolean INVERT_Y = true;
         public static final boolean INVERT_ROTATION = false;
 
         // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
@@ -227,21 +227,22 @@ public final class Constants {
                 .withKS(0).withKV(0.124);
             
             public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
+            public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST
         }
     
         public static final class AutoConstants {
-            public static final Gains XY_GAINS = new Gains(3,0.01,0);
+            public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
             public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
 
             public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
 
             public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
-            public static final int LIDAR_DIO_CHANNEL = 0;
+            public static final int LIDAR_DIO_CHANNEL = 2;
             public static final int LIDAR_MICROS_TO_CM = 10;
             public static final int SECONDS_TO_MICROS = 1000000;
                     
-            public static final double XY_TOLERANCE = 0.1;
+            public static final double XY_TOLERANCE = 0.07; // Meters
-            public static final double ROT_TOLERANCE = 1;
+            public static final double ROT_TOLERANCE = 1; // Degrees
                     
             // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
             // public static final Pose2d targetpos = 
@@ -249,8 +250,8 @@ public final class Constants {
     
     
         public static final class Configurations {
-            public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help.
+            public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
-            public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help.
+            public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
             public static final double NEUTRAL_DEADBAND = 0.04;
 
             // POWER! (limiting)
@@ -282,7 +283,7 @@ public final class Constants {
                     //     new ClosedLoopRampsConfigs()
                     //         .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
             );
-            private static final double SLIP_CURRENT = 100; // TODO: Tune??? 
+            public static final double SLIP_CURRENT = 60; // TODO: Tune??? 
         }
 
         // No mans land
@@ -339,9 +340,11 @@ public final class Constants {
       }
     
     public static final class VisionConstants { 
-        public static final String CAMERA_NAME = "Camera_Module_v1";
+        public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
+        public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
 
-        public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI));
+        public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI));
+        public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI));
         
         public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
 
@@ -354,8 +357,9 @@ public final class Constants {
 
     public static final class FieldConstants {
         public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
-        public static final double HORISONTAL_SCORING_POSITION_OFFSET = .2794
+        // public static final double HORISONTAL_SCORING_POSITION_OFFSET =  Units.inchesToMeters(9.5);
-        ;
+        public static final double HORISONTAL_SCORING_POSITION_OFFSET =  Units.inchesToMeters(6.5);
+        ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
         // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
 
         public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);

src/main/java/frc4388/robot/RobotContainer.java

@@ -36,7 +36,9 @@ import edu.wpi.first.wpilibj2.command.Commands;
 
 // Autos
 import frc4388.utility.controller.VirtualController;
-import frc4388.robot.commands.GotoPositionCommand;
+import frc4388.robot.commands.GotoLastApril;
+import frc4388.robot.commands.LidarAlign;
+import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
 
@@ -73,7 +75,7 @@ public class RobotContainer {
     
     /* Subsystems */
     // private final LED m_robotLED = new LED();
-    public final Vision m_vision = new Vision(m_robotMap.camera);
+    public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
     public final Lidar m_lidar = new Lidar();
     public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
@@ -105,7 +107,26 @@ public class RobotContainer {
     () -> autoplaybackName.get(), // lastAutoName
            new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
            true, false);
-    private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision);
+    private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
+    private Command AprilLidarAlign = new SequentialCommandGroup(
+        new GotoLastApril(m_robotSwerveDrive, m_vision),
+        new InstantCommand(() -> System.out.println("Soup")),
+        new WaitCommand(1),
+        new LidarAlign(m_robotSwerveDrive, m_lidar)
+    );
+    private Command AprilLidarLeft = new SequentialCommandGroup(
+        AutoGotoPosition.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_lidar)
+    );
+
+    private Command AprilLidarRight = new SequentialCommandGroup(
+        AutoGotoPosition.asProxy(),
+        new InstantCommand(() -> System.out.println("Soup")),
+        new WaitCommand(1),
+        new LidarAlign(m_robotSwerveDrive, m_lidar)//,
+        // new MoveForTimeCommand(m_robotSwerveDrive, 
+        // new Translation2d(0, 0.5), new Translation2d(), 1000, true)
+    );
 
     private Command placeCoral = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
@@ -131,7 +152,7 @@ public class RobotContainer {
     public RobotContainer() {
         
         NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition);
-        NamedCommands.registerCommand("april-allign", AutoGotoPosition);
+        NamedCommands.registerCommand("april-allign", AprilLidarAlign);
         NamedCommands.registerCommand("place-coral", placeCoral);
         NamedCommands.registerCommand("grab-coral", grabCoral);
 
@@ -208,6 +229,15 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
 
+        new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // final
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) // final
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.deactivateLuigiMode()));
+
+        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
 
@@ -217,10 +247,11 @@ public class RobotContainer {
         // ?  /* Operator Buttons */
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(AutoGotoPosition);
+            // .onTrue(AutoGotoPosition);
+            .onTrue(AprilLidarRight);
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); 
+            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); 
 
         
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)

src/main/java/frc4388/robot/RobotMap.java

@@ -35,7 +35,8 @@ public class RobotMap {
     // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
     // public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
-    public PhotonCamera camera = new PhotonCamera(VisionConstants.CAMERA_NAME);
+    public PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
+    public PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
     
     public RobotMap() {
         configureDriveMotorControllers();

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -15,7 +15,7 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
 import frc4388.utility.UtilityStructs.AutoRecordingFrame;
 import frc4388.utility.controller.VirtualController;
 
-public class GotoPositionCommand extends Command {
+public class GotoLastApril extends Command {
     
 
     // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
@@ -34,12 +34,18 @@ public class GotoPositionCommand extends Command {
      * @param SwerveDrive m_robotSwerveDrive
      */
 
-    public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
+    public GotoLastApril(SwerveDrive swerveDrive, Vision vision) {
         this.swerveDrive = swerveDrive;
         this.vision = vision;
         // addRequirements(swerveDrive);
     }
 
+
+    public static double tagRelativeXError = -1;
+    private static void setTagRelativeXError(double val){
+        tagRelativeXError = val;
+    }
+
     @Override
     public void initialize() {
         xPID.initialize();
@@ -65,8 +71,8 @@ public class GotoPositionCommand extends Command {
         double rotoutput = rotPID.update(roterr);
 
         Translation2d leftStick = new Translation2d(
-            Math.max(Math.min(youtput, 1), -1),
+            Math.max(Math.min(-youtput, 1), -1),
-            Math.max(Math.min(xoutput, 1), -1)
+            Math.max(Math.min(-xoutput, 1), -1)
             // 0,0
         );
 
@@ -75,9 +81,10 @@ public class GotoPositionCommand extends Command {
            0
         );
 
-        // SmartDashboard.putNumber("PID X Output", xoutput);
+        setTagRelativeXError(
-        // SmartDashboard.putNumber("PID Y Output", youtput);
+            new Translation2d(xerr, yerr).getAngle()
-        // // SmartDashboard.putNumber("PID Y Output", youtput);
+            .rotateBy(targetpos.getRotation())
+            .getCos());
 
         swerveDrive.driveWithInput(leftStick, rightStick, true);
     }
@@ -86,6 +93,10 @@ public class GotoPositionCommand extends Command {
     public final boolean isFinished() {
         boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
         // System.out.println(finished);
+
+        if(finished)
+            swerveDrive.softStop();
+
         return finished;
                 // this statement is a boolean in and of itself
     }

src/main/java/frc4388/robot/commands/LidarAlign.java

@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Lidar;
+import frc4388.robot.subsystems.SwerveDrive;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class LidarAlign extends Command {
+  private SwerveDrive swerveDrive;
+  private Lidar lidar;  
+
+  private int currentFinderTick;
+  // private int tickFoundPipe;
+  private boolean foundReef;
+  private boolean headedRight;
+  private double speed;
+  // private final boolean constructedHeadedRight;
+
+  /** Creates a new LidarAlign. */
+  public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.swerveDrive = swerveDrive;
+    this.lidar = lidar;
+
+    addRequirements(swerveDrive, lidar);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.currentFinderTick = 0;
+    this.speed = 0.4; // TODO: find good speed for this
+    this.foundReef = false;
+    this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
+  }
+
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (lidar.withinDistance()) {
+      swerveDrive.softStop(); 
+      foundReef = true;
+      return;
+    }
+
+    if (currentFinderTick > 100) { //arbutrary threshhold for now.
+      headedRight = !headedRight;
+      currentFinderTick *= -1;
+    }
+    double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
+    double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
+    SmartDashboard.putNumber("Rel Angle", relAngle);
+    SmartDashboard.putNumber("heading", currentHeading);
+    if (!headedRight) {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
+    } else {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
+    }
+
+    currentFinderTick++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (foundReef && lidar.withinDistance()) { // spot on
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && !lidar.withinDistance()) { // over shot
+      speed = speed / 2;
+      headedRight = !headedRight;
+      currentFinderTick = 0;
+      foundReef = false;
+      return false;
+    } else {
+      return false;
+    }
+  }
+}

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -0,0 +1,48 @@
+package frc4388.robot.commands;
+
+import java.time.Instant;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.SwerveDrive;
+
+// Command to repeat a joystick movement for a specific time.
+public class MoveForTimeCommand extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final long duration;
+    private final boolean robotRelative;
+
+    private Instant startTime;
+
+    public MoveForTimeCommand(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        long millis,
+        boolean robotRelative) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.duration = millis;
+        this.robotRelative = robotRelative;
+    }
+
+    @Override
+    public void initialize() {
+        startTime = Instant.now();
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
+    }
+}

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -24,9 +24,14 @@ public class Elevator extends SubsystemBase {
   public enum CoordinationState {
     Waiting, // for coral into the though 
     Ready, // Has coral in enefector
+    PrimedThree, // Arm and elevator Waiting to score in the level 3 position
     ScoringThree, // Arm and elevator in the level three position
-    ScoringFour // Arm and elevator in the level four position
+    PrimedFour, // Arm and elevator Waiting to score in the level 4 position
-
+    ScoringFour, // Arm and elevator in the level four position
+    BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef.
+    BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef.
+    BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef.
+    BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef.
   }
 
   private CoordinationState currentState;

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -8,10 +8,13 @@ import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+import com.ctre.phoenix6.swerve.SwerveModule;
 import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -28,6 +31,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.commands.GotoLastApril;
+import frc4388.robot.commands.LidarAlign;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
 import frc4388.utility.Status.ReportLevel;
@@ -149,6 +154,7 @@ public class SwerveDrive extends Subsystem {
                     .withVelocityX(leftStick.getX() * speedAdjust)
                     .withVelocityY(leftStick.getY() * speedAdjust)
                     .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+                    // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
                 SmartDashboard.putBoolean("drift correction", false);
             } else {
                 var ctrl = new SwerveRequest.FieldCentricFacingAngle()
@@ -210,6 +216,41 @@ public class SwerveDrive extends Subsystem {
                 .withTargetDirection(rightStick.getAngle()));
     }
 
+    public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
+        leftStick = leftStick.rotateBy(heading);
+
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withTargetDirection(Rotation2d.fromDegrees(rotTarget));
+        ctrl.HeadingController.setPID(
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        );
+        swerveDriveTrain.setControl(ctrl);
+    }
+
+    public void setLimits(double limitInAmps) {
+        for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
+            var talonFXConfigurator = module.getDriveMotor().getConfigurator();
+            var talonFXConfigs = new TalonFXConfiguration();
+
+            talonFXConfigurator.refresh(talonFXConfigs);
+            talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
+            talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
+            talonFXConfigurator.apply(talonFXConfigs);
+        }
+    }
+
+    public void activateLuigiMode() {
+        setLimits(20);
+    }
+
+    public void deactivateLuigiMode() {
+        setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
+    }
+
     public boolean rotateToTarget(double angle) {
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(0)
@@ -241,13 +282,27 @@ public class SwerveDrive extends Subsystem {
     }
 
     public double getGyroAngle() {
-        return swerveDriveTrain.getRotation3d().getAngle();
+        return getPose2d().getRotation().getRadians();
+    }
+
+    public Pose2d getPose2d() {
+        return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
     }
 
     public void resetGyro() {
         swerveDriveTrain.tareEverything();
     }
 
+
+    public void softStop() {
+        stopped = true;
+        swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+            .withVelocityX(0)
+            .withVelocityY(0)
+            .withRotationalRate(0)
+        ); // stop the modules without breaking
+    }
+
     public void stopModules() {
         stopped = true;
         swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
@@ -256,11 +311,12 @@ public class SwerveDrive extends Subsystem {
     @Override
     public void periodic() {
         // This method will be called once per scheduler run\
-        SmartDashboard.putNumber("Gyro", getGyroAngle());
+        SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
         SmartDashboard.putNumber("RotTartget", rotTarget);
 
         double time = Vision.getTime();
         
+
         vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
 
         if (vision.isTag()) {

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -39,7 +39,11 @@ import frc4388.utility.Subsystem;
 
 public class Vision extends Subsystem {
 
-    private PhotonCamera camera;
+    // private PhotonCamera leftCamera;
+    // private PhotonCamera rightCamera;
+
+    private PhotonCamera[] cameras;
+    private PhotonPoseEstimator[] estimators;
 
     private boolean isTagDetected = false;
     private boolean isTagProcessed = false;
@@ -47,7 +51,6 @@ public class Vision extends Subsystem {
     private Pose2d lastPhysOdomPose = new Pose2d();
 
     private Matrix<N3, N1> curStdDevs;
-    private final PhotonPoseEstimator photonEstimator;
 
     private Field2d field = new Field2d();
 
@@ -66,59 +69,87 @@ public class Vision extends Subsystem {
     .withWidget(BuiltInWidgets.kBooleanBox)
     .getEntry();
     
-    GenericEntry sbCamConnected = subsystemLayout
+    GenericEntry sbLeftCamConnected = subsystemLayout
-    .add("Camera Connnected", false)
+    .add("Left Camera Connnected", false)
     .withWidget(BuiltInWidgets.kBooleanBox)
     .getEntry();
 
-    public Vision(PhotonCamera camera){
+    GenericEntry sbRightCamConnected = subsystemLayout
-        this.camera = camera;
+    .add("Right Camera Connnected", false)
+    .withWidget(BuiltInWidgets.kBooleanBox)
+    .getEntry();
+    
+    public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){
         SmartDashboard.putData(field);
 
-        photonEstimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS);
+        this.cameras = new PhotonCamera[]{leftCamera, rightCamera};
-        photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+
+        PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS);
+        PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS);
+
+        photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+        photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+
+        this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight};
     }
 
     @Override
     public void periodic() {
-        // var result = camera.getLatestResult();
+        update();
-        var results = camera.getAllUnreadResults();
-        if (results.size() == 0) return;
-        var result = results.get(results.size()-1);
-
-        isTagDetected = result.hasTargets();
-        isTagProcessed = false;
-        
-
-        if(!isTagDetected){
-            // sbTagDetected.setBoolean(isTagDetected);
-            field.setRobotPose(getPose2d());
-            return;
-        }
-
-        var EstimatedRobotPose = getEstimatedGlobalPose(result);
-
-        // In case the pose estimator fails to estimate the pose, fallback to physical odometry.
-        if(EstimatedRobotPose.isEmpty()){
-            field.setRobotPose(getPose2d());
-            return;
-        }
-
-        isTagProcessed = true;
-
-        lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
-        // lastVisionPose.rotateBy(Rotation2d.k180deg);
-        // lastVisionPose = new Pose2d(
-        //     lastVisionPose.getTranslation(),
-        //     lastPhysOdomPose.getRotation()
-        // );
-
-        
-        
         field.setRobotPose(getPose2d());
     }
 
 
+    private void update() {
+        isTagProcessed = false;
+        isTagDetected = false;
+
+
+        int cams = 0;
+
+        double X = 0;
+        double Y = 0;
+        double Yaw = 0;
+
+        for(int i = 0; i < cameras.length; i++){
+            PhotonCamera camera = cameras[i];
+            PhotonPoseEstimator estimator = estimators[i];
+
+            var results = camera.getAllUnreadResults();
+
+            // If there are no more updates from the camera
+            if (results.size() == 0) 
+                continue;
+
+            
+            var result = results.get(results.size()-1);
+
+            isTagDetected = isTagDetected | result.hasTargets();
+
+            // If there are no tags
+            if(!result.hasTargets())
+                continue;
+
+            Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
+
+            // If the tag was failed to be processed
+            if(estimatedRobotPose.isEmpty())
+                continue;
+
+            Pose2d pose = estimatedRobotPose.get().estimatedPose.toPose2d();
+            X += pose.getX();
+            Y += pose.getY();
+            Yaw += pose.getRotation().getDegrees();
+            cams++;
+            isTagProcessed = true;
+        
+            
+        }
+
+        if(isTagProcessed){
+            lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams));
+        }
+    }
 
 
     /**
@@ -131,36 +162,24 @@ public class Vision extends Subsystem {
      * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
      *     used for estimation.
      */
-    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change) {
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
         Optional<EstimatedRobotPose> visionEst = Optional.empty();
-        // for (var change : camera.getAllUnreadResults()) {
 
-            var targets = change.getTargets();
+        var targets = change.getTargets();
-            for(int i = targets.size()-1; i >= 0; i--){
+        for(int i = targets.size()-1; i >= 0; i--){
-                Transform3d pos = targets.get(i).getBestCameraToTarget();
+            Transform3d pos = targets.get(i).getBestCameraToTarget();
-                double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
+            double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
-                if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
+            if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
-                    change.targets.remove(i);
+                change.targets.remove(i);
-                }
             }
+        }
 
-            if(targets.size() <= 0)
+        if(targets.size() <= 0)
-                return visionEst; // Will be empty
+            return visionEst; // Will be empty
 
-            visionEst = photonEstimator.update(change);
+        visionEst = estimator.update(change);
-            updateEstimationStdDevs(visionEst, change.getTargets());
+        updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
 
-            // if (Robot.isSimulation()) {
-            //     visionEst.ifPresentOrElse(
-            //             est ->
-            //                     getSimDebugField()
-            //                             .getObject("VisionEstimation")
-            //                             .setPose(est.estimatedPose.toPose2d()),
-            //             () -> {
-            //                 getSimDebugField().getObject("VisionEstimation").setPoses();
-            //             });
-            // }
-        // }
         return visionEst;
     }
 
@@ -173,7 +192,9 @@ public class Vision extends Subsystem {
      * @param targets All targets in this camera frame
      */
     private void updateEstimationStdDevs(
-            Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
+            Optional<EstimatedRobotPose> estimatedPose, 
+            List<PhotonTrackedTarget> targets,
+            PhotonPoseEstimator estimator) {
         if (estimatedPose.isEmpty()) {
             // No pose input. Default to single-tag std devs
             curStdDevs = VisionConstants.kSingleTagStdDevs;
@@ -186,7 +207,7 @@ public class Vision extends Subsystem {
 
             // Precalculation - see how many tags we found, and calculate an average-distance metric
             for (var tgt : targets) {
-                var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+                var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
                 if (tagPose.isEmpty()) continue;
                 
                 double distance = tagPose
@@ -281,7 +302,8 @@ public class Vision extends Subsystem {
     public void queryStatus() {
         sbTagDetected.setBoolean(isTagDetected);
         sbTagProcessed.setBoolean(isTagProcessed);
-        sbCamConnected.setBoolean(camera.isConnected());
+        sbLeftCamConnected.setBoolean(cameras[0].isConnected());
+        sbRightCamConnected.setBoolean(cameras[1].isConnected());
         // field.setRobotPose(getPose2d());
     }
 

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -50,14 +50,11 @@ public class ReefPositionHelper {
         for(int i = 1; i < locations.length; i++){
             double dist = distanceTo(locations[i],position);
             if(dist < minDistance){
-                System.out.println(i);
                 minPos = locations[i];
                 minDistance = dist;
             }
         }
 
-        System.out.println(minPos);
-
         return minPos;
     }