command reorganization

src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Placement;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveToLimeDistance extends CommandBase {
+  /** Creates a new DriveToLimeDistance. */
+  public DriveToLimeDistance() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // 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;
+  }
+}

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

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Placement;
+
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class LimeAlign extends PelvicInflammatoryDisease {
+
+  SwerveDrive drive;
+  Limelight lime;
+
+  public LimeAlign(SwerveDrive drive, Limelight lime) {
+    super(0.7, 0.4, 0.0, 0.0, 0.04);
+
+    this.drive = drive;
+    this.lime = lime;
+    
+    addRequirements(drive, lime);
+  }
+
+  @Override
+  public double getError() {
+
+    if (lime.numTargets() > 2) {
+      return 0.0;
+    }
+
+    double err = 0.0;
+
+    try {
+      err = lime.getLowestTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); 
+    } catch (NullPointerException ex) {}
+    
+    return err;
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+
+    if (output > 0) {
+      output += 0.6;
+    } else if (output < 0) {
+      output -= 0.6;
+    }
+
+    drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
+  }
+}