Improve lidar align accuracy

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

@@ -250,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)
@@ -283,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
@@ -355,8 +355,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,7 @@ 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.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
@@ -106,7 +106,12 @@ 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 LidarAlign(m_robotSwerveDrive, m_lidar, true)
+    );
     private Command AprilLidarLeft = new SequentialCommandGroup(
         AutoGotoPosition.asProxy(),
         new LidarAlign(m_robotSwerveDrive, m_lidar, false)
@@ -114,6 +119,7 @@ public class RobotContainer {
 
     private Command AprilLidarRight = new SequentialCommandGroup(
         AutoGotoPosition.asProxy(),
+        new InstantCommand(() -> System.out.println("Soup")),
         new LidarAlign(m_robotSwerveDrive, m_lidar, true)
     );
 
@@ -141,7 +147,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);
 
@@ -218,6 +224,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()));
 

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,7 +34,7 @@ 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);

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

@@ -6,6 +6,7 @@ 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;
@@ -56,8 +57,10 @@ public class LidarAlign extends Command {
       headedRight = !headedRight;
       currentFinderTick *= -1;
     }
-
+    double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
-    double relAngle = (Math.round(swerveDrive.getGyroAngle() / 60.d) * 60) + 90; // Relative driving to the side of the reef
+    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 {

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();