finished untuned aprilTag locking

src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java

@@ -4,41 +4,36 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class CompControl implements ControlBoard {
-    private CommandXboxController driver = new CommandXboxController(0);
-    private CommandXboxController operator = new CommandXboxController(1);
-
-    // TODO: Implement operator controls in the future
-    // =============Operator Controls=============
-
-    // =============Driver Controls=============
-    @Override
-    public double xInput() {
-        // TODO Auto-generated method stub
-        return this.driver.getLeftX();
-    }
-
-    @Override
-    public double yInput() {
-        // TODO Auto-generated method stub
-        return this.driver.getLeftY();
-    }
-
-    @Override
-    public double rotationalInput() {
-        // TODO Auto-generated method stub
-        return this.driver.getRightX();
-    }
-
-    @Override
-    public Trigger brake() {
-        // TODO Auto-generated method stub
-        return this.driver.leftBumper();
-    }
-
-    @Override
-    public Trigger setZeroHeading() {
-        // TODO Auto-generated method stub
-        return this.driver.rightBumper();
-    }
+	private CommandXboxController driver = new CommandXboxController(0);
+	private CommandXboxController operator = new CommandXboxController(1);
+
+	// TODO: Implement operator controls in the future
+	// =============Operator Controls=============
+
+	// =============Driver Controls=============
+	@Override
+	public double xInput() {
+		return this.driver.getLeftX();
+	}
+
+	@Override
+	public double yInput() {
+		return this.driver.getLeftY();
+	}
+
+	@Override
+	public double rotationalInput() {
+		return this.driver.getRightX();
+	}
+
+	@Override
+	public Trigger brake() {
+		return this.driver.leftBumper();
+	}
+
+	@Override
+	public Trigger setZeroHeading() {
+		return this.driver.rightBumper();
+	}
 
 }

src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/LockAprilTag.java

@@ -2,24 +2,32 @@
 
 import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
+//TODO: move all hardcoded numbers to constants file
 public class LockAprilTag extends Command {
 	private DriveSubsystem drive;
 	private VisionSubsystem vision;
 
 	private ProfiledPIDController lockPID;
 	private Constraints lockPIdConstraints;
 	private Pose2d currentPose;
-	private double currentAngle;
+	private double currentYaw;
 
 	private double fiducialID;
 
+	private PhotonTrackedTarget target;
+
+	private double angleGoal;
+
 	public LockAprilTag(DriveSubsystem drive, VisionSubsystem vision, double fiducialID) {
 		this.drive = drive;
 		this.vision = vision;
@@ -28,25 +36,47 @@ public LockAprilTag(DriveSubsystem drive, VisionSubsystem vision, double fiducia
 		this.lockPID = new ProfiledPIDController(0.3, 0, 0, this.lockPIdConstraints);
 
 		this.fiducialID = fiducialID;
+		this.target = this.vision.getTarget(this.fiducialID);
+
+		this.angleGoal = this.target.getYaw();
+
+		SmartDashboard.putData("LockPID", this.lockPID);
 		addRequirements(this.drive, this.vision);
 	}
 
 	@Override
 	public void initialize() {
-		this.lockPID.reset(new State(0, 0));
-		this.lockPID.setGoal(0);
+		this.lockPID.reset(new State(this.angleGoal, 0));
+		this.lockPID.setGoal(this.angleGoal);
+		this.lockPID.setTolerance(1, 1);
+
+		this.drive.stopDrive();
 	}
 
 	@Override
 	public void execute() {
 		this.currentPose = this.drive.getPose();
-		this.currentAngle = this.drive.getPose().getRotation().getDegrees();
+		this.currentYaw = this.drive.getPose().getRotation().getDegrees();
+
+		this.lockPID.setConstraints(this.lockPIdConstraints);
+
+		var rawOutput = this.lockPID.calculate(this.currentYaw);
+		double output = MathUtil.clamp(rawOutput, -0.5, 0.5);
+
+		this.drive.drive(0, 0, output, true, false);
+
+		SmartDashboard.putNumber("LockPID/macAcc", this.lockPIdConstraints.maxAcceleration);
+		SmartDashboard.putNumber("LockPID/maxVel", this.lockPIdConstraints.maxVelocity);
+		SmartDashboard.putNumber("LockPID/PositionError", this.lockPID.getPositionError());
+		SmartDashboard.putNumber("LockPID/VelocityError", this.lockPID.getVelocityError());
+		SmartDashboard.putNumber("LockPID/output", output);
+		SmartDashboard.putNumber("LockPID/currentYaw", currentYaw);
 
 	}
 
 	@Override
 	public boolean isFinished() {
-		return false;
+		return this.lockPID.atGoal();
 	}
 
 	@Override

src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/swerve/RevSwerveDrive.java

@@ -77,13 +77,11 @@ public double getHeading() {
 
 	@Override
 	public void stopMotor() {
-		// TODO Auto-generated method stub
 		throw new UnsupportedOperationException("Unimplemented method 'stopMotor'");
 	}
 
 	@Override
 	public String getDescription() {
-		// TODO Auto-generated method stub
 		throw new UnsupportedOperationException("Unimplemented method 'getDescription'");
 	}
 

src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java

@@ -48,6 +48,7 @@ public class VisionSubsystem extends SubsystemBase implements Logged {
 	private DriveSubsystem drive;
 
 	private double[] flucialIDs;
+	List<PhotonTrackedTarget> targets;
 
 	public VisionSubsystem(DriveSubsystem drive) {
 		this.cam = new PhotonCamera("Sechenov");
@@ -60,7 +61,6 @@ public VisionSubsystem(DriveSubsystem drive) {
 		try {
 			layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile);
 		} catch (IOException e) {
-			// TODO: error handling
 			System.out.println(e);
 			DriverStation.reportError("Fail to load the april tag map", e.getStackTrace());
 		}
@@ -75,7 +75,7 @@ public VisionSubsystem(DriveSubsystem drive) {
 	public void periodic() {
 		PhotonPipelineResult results = this.cam.getLatestResult();
 
-		List<PhotonTrackedTarget> targets = results.getTargets();
+		targets = results.getTargets();
 
 		SmartDashboard.putBoolean("Vision/isConnected", this.cam.isConnected());
 		int len = targets.size();
@@ -105,6 +105,14 @@ public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevPose) {
 		return this.estimator.update();
 	}
 
+	public PhotonTrackedTarget getTarget(double fiducialID) {
+		for (PhotonTrackedTarget i : targets) {
+			if (i.getFiducialId() == fiducialID) {
+				return i;
+			}
+		}
+		return null;
+	}
 	VisionSystemSim visionSim = new VisionSystemSim("main");
 	PhotonCameraSim cameraSim;