lidar align works

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

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

@@ -37,6 +37,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
 // Autos
 import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.GotoPositionCommand;
+import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
 
@@ -106,6 +107,15 @@ public class RobotContainer {
            new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
            true, false);
     private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision);
+    private Command AprilLidarLeft = new SequentialCommandGroup(
+        AutoGotoPosition.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_lidar, false)
+    );
+
+    private Command AprilLidarRight = new SequentialCommandGroup(
+        AutoGotoPosition.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_lidar, true)
+    );
 
     private Command placeCoral = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
@@ -217,7 +227,8 @@ 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)); 

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

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

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

@@ -37,7 +37,7 @@ public class LidarAlign extends Command {
   @Override
   public void initialize() {
     this.currentFinderTick = 0;
-    this.speed = 0.1; // TODO: find good speed for this
+    this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
     this.headedRight = constructedHeadedRight;
   }
@@ -57,11 +57,11 @@ public class LidarAlign extends Command {
       currentFinderTick *= -1;
     }
 
-    double relAngle = Math.round(swerveDrive.getGyroAngle() / 60.d) * 60; // Relative driving to the side of the reef
+    double relAngle = (Math.round(swerveDrive.getGyroAngle() / 60.d) * 60) + 90; // Relative driving to the side of the reef
     if (!headedRight) {
-      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
-    } else {
       swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
+    } else {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
     }
 
     currentFinderTick++;

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

@@ -12,6 +12,7 @@ import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
 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;
@@ -149,6 +150,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()