lime align hopefully done

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

@@ -144,7 +144,7 @@ public final class Constants {
   }
 
   public static final class VisionConstants {
-    public static final String NAME = "Aarav is poopy";
+    public static final String NAME = "photonCamera";
 
     public static final int LIME_HIXELS = 640;
     public static final int LIME_VIXELS = 480;

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

@@ -7,10 +7,6 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-
-import edu.wpi.first.wpilibj.PWM;
-import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -21,6 +17,7 @@ import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.commands.JoystickPlayback;
 import frc4388.robot.subsystems.Arm;
 import frc4388.robot.subsystems.Claw;
+import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.commands.JoystickRecorder;
 import frc4388.robot.commands.PivotCommand;
@@ -50,6 +47,8 @@ public class RobotContainer {
 
     public final Claw m_robotClaw = new Claw(m_robotMap.servo);
 
+    public final Limelight m_limeLight = new Limelight();
+
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);

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

@@ -38,16 +38,4 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 		if (Math.abs(getError()) < 3) out2 = 0;
 		drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
 	}
-
-	@Override
-	public void initialize() {
-		super.initialize();
-		// this.gyro.reset();
-	}
-
-	// Returns true when the command should end.
-	@Override
-	public boolean isFinished() {
-		return false;
-	}
 }

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

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

@@ -4,7 +4,6 @@
 
 package frc4388.robot.commands;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.utility.Gains;
 
@@ -26,12 +25,13 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 
 	/** produces the error from the setpoint */
 	public abstract double getError();
+	
 	/** figure it out bitch */
 	public abstract void runWithOutput(double output);
 
 	// Called when the command is initially scheduled.
 	@Override 
-	public void initialize() {
+	public final void initialize() {
 		output = 0;
 	}
 
@@ -39,7 +39,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 	
 	// Called every time the scheduler runs while the command is scheduled.
 	@Override 
-	public void execute() {
+	public final void execute() {
 		double error = getError();
 		cumError += error * .02; // 20 ms
 		double delta = error - prevError;
@@ -54,7 +54,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 
 	// Returns true when the command should end.
 	@Override
-	public boolean isFinished() {
+	public final boolean isFinished() {
 		return Math.abs(getError()) < tolerance;
 	}
 }

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

@@ -14,14 +14,14 @@ import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
-import edu.wpi.first.math.Pair;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.utility.AbhiIsADumbass;
 
 public class Limelight extends SubsystemBase {
   private PhotonCamera cam;
-  /** Creates a new LImelight. */
+
+  /** Creates a new Limelight. */
   public Limelight() {
     cam = new PhotonCamera(VisionConstants.NAME);
   }