Improve lidar align accuracy

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,9 +8,11 @@ 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;
 
@@ -227,6 +229,26 @@ public class SwerveDrive extends Subsystem {
         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)
@@ -258,7 +280,11 @@ 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() {
@@ -283,7 +309,7 @@ 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();