Limelight lidar stuff

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

@@ -40,6 +40,12 @@ public class GotoLastApril extends Command {
         // addRequirements(swerveDrive);
     }
 
+
+    public static double tagRelativeXError = -1;
+    private static void setTagRelativeXError(double val){
+        tagRelativeXError = val;
+    }
+
     @Override
     public void initialize() {
         xPID.initialize();
@@ -75,17 +81,10 @@ public class GotoLastApril extends Command {
            0
         );
 
-        Rotation2d error = new Translation2d(xerr, yerr).getAngle();
-
-        error = error.rotateBy(Rotation2d.fromDegrees(-rotoutput));
-
-        double tagRelativeXError = error.getSin();
-
-        System.out.println(tagRelativeXError);
-
-        // SmartDashboard.putNumber("PID X Output", xoutput);
-        // SmartDashboard.putNumber("PID Y Output", youtput);
-        // // SmartDashboard.putNumber("PID Y Output", youtput);
+        setTagRelativeXError(
+            new Translation2d(xerr, yerr).getAngle()
+            .rotateBy(targetpos.getRotation())
+            .getCos());
 
         swerveDrive.driveWithInput(leftStick, rightStick, true);
     }

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

@@ -21,12 +21,11 @@ public class LidarAlign extends Command {
   private boolean foundReef;
   private boolean headedRight;
   private double speed;
-  private final boolean constructedHeadedRight;
+  // private final boolean constructedHeadedRight;
 
   /** Creates a new LidarAlign. */
-  public LidarAlign(SwerveDrive swerveDrive, Lidar lidar, boolean headedRight) {
+  public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) {
     // Use addRequirements() here to declare subsystem dependencies.
-    constructedHeadedRight = headedRight;
 
     this.swerveDrive = swerveDrive;
     this.lidar = lidar;
@@ -40,7 +39,7 @@ public class LidarAlign extends Command {
     this.currentFinderTick = 0;
     this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
-    this.headedRight = constructedHeadedRight;
+    this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
   }
 
 

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

@@ -22,6 +22,7 @@ public class MoveForTimeCommand extends Command {
         Translation2d rightStick, 
         long millis,
         boolean robotRelative) {
+            addRequirements(swerveDrive);
 
         this.swerveDrive = swerveDrive;
         this.leftStick = leftStick;