[#108] chase pieces

src/main/java/frc/robot/Constants.java

@@ -283,6 +283,7 @@ public class VisionConstants {
         public static final double COLLISION_DEADZONE = 2d;
         public static final double ALIGNMENT_TOLERANCE = 4d; // TODO: make this an actual value
         public static final PIDController HEADING_CONTROLLER = new PIDController(0.05, 0, 0);
+        public static final PIDController CHASE_CONTROLLER = new PIDController(0.12, 0, 0.05);
         public static final int TAG_PIPELINE = 0;
         public static final int NOTE_PIPELINE = 2;
     }

src/main/java/frc/robot/RobotContainer.java

@@ -28,6 +28,7 @@
 import frc.robot.Constants.TunerConstants;
 import frc.robot.command.PointAtTag;
 import frc.robot.command.Shoot;
+import frc.robot.command.ChasePieces;
 import frc.robot.command.Climb;
 import frc.robot.command.ManualClimb;
 import frc.robot.subsystems.Climber;
@@ -102,9 +103,10 @@ protected void configureButtonBindings() { //TODO decide on comp buttons
 
 		new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false)));
 
-		new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", true, true));
+		new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain));
 		new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain));
 
+
 		// new Trigger(driver::getYButton).whileTrue(new Climb(climber, drivetrain));
 	}
 

src/main/java/frc/robot/command/ChasePieces.java

@@ -0,0 +1,92 @@
+package frc.robot.command;
+
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.Constants.VisionConstants;
+import frc.robot.subsystems.Swerve;
+import frc.thunder.shuffleboard.LightningShuffleboard;
+import frc.thunder.vision.Limelight;
+
+public class ChasePieces extends Command {
+
+	private Swerve drivetrain;
+	private Limelight limelight;
+
+	private RobotCentric noteChase;
+
+	private int limelightId = 0;
+	private double pidOutput;
+	private double targetHeading;
+    private boolean onTarget;
+	private PIDController headingController = VisionConstants.CHASE_CONTROLLER;
+
+	/**
+	 * Creates a new ChasePieces.
+	 * @param drivetrain to request movement 
+	 */
+	public ChasePieces(Swerve drivetrain) {
+		this.drivetrain = drivetrain;
+
+		limelight = drivetrain.getLimelights()[0];
+		limelightId = limelight.getPipeline();
+
+		limelight.setPipeline(VisionConstants.NOTE_PIPELINE);
+
+		noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+
+	}
+
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {
+		headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE);
+	}
+
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override
+	public void execute() {
+
+		targetHeading = limelight.getTargetX();
+
+        if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){
+            onTarget = true;
+        } else{
+            onTarget = false;
+        }
+
+		LightningShuffleboard.setBool("ChasePieces", "On Target", onTarget);
+		LightningShuffleboard.setDouble("ChasePieces", "Drivetrain Angle", drivetrain.getPigeon2().getAngle());
+		LightningShuffleboard.setDouble("ChasePieces", "Target Heading", targetHeading);
+		LightningShuffleboard.setDouble("ChasePieces", "Pid Output", pidOutput);
+
+		headingController.setP(LightningShuffleboard.getDouble("ChasePieces", "Pid P", 0.1));
+		headingController.setI(LightningShuffleboard.getDouble("ChasePieces", "Pid I", 0));
+		headingController.setD(LightningShuffleboard.getDouble("ChasePieces", "Pid D", 0));
+
+
+		pidOutput = headingController.calculate(0, targetHeading);
+
+        drivetrain.setControl(noteChase.withRotationalRate(-pidOutput) 
+		.withVelocityX(1.5)
+		);
+
+	}
+
+	// Called once the command ends or is interrupted.
+	@Override
+	public void end(boolean interrupted) {
+		limelight.setPipeline(limelightId);
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

src/main/java/frc/robot/command/PointAtTag.java

@@ -90,6 +90,9 @@ public void execute() {
 		LightningShuffleboard.setDouble("PointAtTag", "Target Heading", targetHeading);
 		LightningShuffleboard.setDouble("PointAtTag", "Pid Output", pidOutput);
 
+
+		pidOutput = headingController.calculate(0, targetHeading);
+
 		if (driver.getRightBumper()) {
 			drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_
 				.withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left)