make lidar align theoretically work

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

@@ -227,6 +227,7 @@ 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 {

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

@@ -4,6 +4,8 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.SwerveDrive;
@@ -17,6 +19,7 @@ public class LidarAlign extends Command {
   // private int tickFoundPipe;
   private boolean foundReef;
   private boolean headedRight;
+  private double speed;
   private final boolean constructedHeadedRight;
 
   /** Creates a new LidarAlign. */
@@ -26,12 +29,15 @@ public class LidarAlign extends Command {
 
     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.05; // TODO: find good speed for this
     this.foundReef = false;
     this.headedRight = constructedHeadedRight;
   }
@@ -40,18 +46,47 @@ public class LidarAlign extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double speed = 0.05; // TODO: find good speed for this
+    if (lidar.withinDistance()) {
+      swerveDrive.softStop(); 
+      foundReef = true;
+      return;
+    }
+
+    if (currentFinderTick > 100) { //arbutrary threshhold for now.
+      headedRight = !headedRight;
+      currentFinderTick *= -1;
+    }
+
     double relAngle = Math.round(swerveDrive.getGyroAngle() / 60.d) * 60; // 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));
+    }
+
+    currentFinderTick++;
   }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+
+  }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    if (foundReef && lidar.withinDistance()) {
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && !lidar.withinDistance()) {
+      speed = speed / 2;
+      headedRight = !headedRight;
+      currentFinderTick = 0;
+      foundReef = false;
+      return false;
+    } else {
+      return false;
+    }
   }
 }

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

@@ -210,6 +210,21 @@ 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 boolean rotateToTarget(double angle) {
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(0)
@@ -248,6 +263,16 @@ public class SwerveDrive extends Subsystem {
         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());