lime align hopefully done

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

@@ -4,31 +4,39 @@
 
 package frc4388.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.AbhiIsADumbass;
 
-public class LimeAlign extends CommandBase {
-  public LimeAlign(SwerveDrive drive) {
-    addRequirements(drive);
-  }
+public class LimeAlign extends PelvicInflammatoryDisease {
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
+  SwerveDrive drive;
+  Limelight lime;
+
+
+  public LimeAlign(SwerveDrive drive, Limelight lime) {
+    super(0.1, 0.0, 0.0, 0.0, 10);
+
+    this.drive = drive;
+    this.lime = lime;
     
+    addRequirements(drive, lime);
   }
 
-  // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public double getError() {
+    try {
+      return lime.getTargetPoints().get(0).x - (VisionConstants.LIME_HIXELS / 2);
+    } catch (AbhiIsADumbass abhiIsADumbass) {
+      abhiIsADumbass.printStackTrace();
+      return 0;
+    }
+  }
 
-  // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
+  public void runWithOutput(double output) {
+    drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
   }
 }