Merge branch 'main' into 101-more-logging

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

@@ -283,6 +283,8 @@ 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 int TAG_PIPELINE = 0;
+        public static final int NOTE_PIPELINE = 2;
     }
 
     public class CollectorConstants { // TODO: get real

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

@@ -102,12 +102,12 @@ protected void configureButtonBindings() { //TODO decide on comp buttons
 				.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.SLOW_ROT_MULT))); // Drive counterclockwise with negative X (left)
 
 		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", false));
+
+		new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", true, true));
 		new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain));
-		new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", false));
 
 		new Trigger(driver::getYButton).whileTrue(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED));
-	}
+  }
 
 	@Override
 	protected void initializeNamedCommands() {

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

@@ -20,6 +20,7 @@ public class PointAtTag extends Command {
 
 	private Swerve drivetrain;
 	private Limelight limelight;
+
 	private XboxController driver;
 	private FieldCentric slow;
 	private FieldCentric drive;
@@ -40,19 +41,29 @@ public class PointAtTag extends Command {
 	 * @param driver the driver's controller, used for drive input
 	 * @param limelight_name the name of the limelight to use
 	 * @param useLimelights to get if we want to use vision data or not
+	 * @param noteDetection to get if we want to use this for note detection or april tag detection
 	 */
-	public PointAtTag(Swerve drivetrain, XboxController driver, String limelight_name, boolean useLimelights) {
+	public PointAtTag(Swerve drivetrain, XboxController driver, String limelight_name, boolean useLimelights, boolean noteDetection) {
 		this.drivetrain = drivetrain;
 		this.driver = driver;
 		this.useLimelights = useLimelights;
 
+
 		//TODO Figure out which of these is the right one to use 
-		for (var l : drivetrain.getLimelights()) { // THIS WAS ON THIS BRANCH
-			if (l.getName().equals(limelight_name)) {
-				limelight = l;
+		for (var l : drivetrain.getLimelights()) { 
+		 	if (l.getName().equals(limelight_name)) {
+		 		limelight = l;
 			}
 		}
-		limelights = drivetrain.getLimelights(); // THIS IS THE OTHER ONE FROM MAIN
+
+		limelight = drivetrain.getLimelights()[0];
+
+		limelightId = limelight.getPipeline();
+		if (noteDetection){
+			limelight.setPipeline(VisionConstants.NOTE_PIPELINE);
+		} else {
+			limelight.setPipeline(VisionConstants.TAG_PIPELINE);
+		}
 
 		drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop
 		slow = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -62,28 +73,21 @@ public PointAtTag(Swerve drivetrain, XboxController driver, String limelight_nam
 	@Override
 	public void initialize() {
 		headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE);
-		limelightId = limelight.getPipeline();
-		limelight.setPipeline(2);
 	}
 
 	// Called every time the scheduler runs while the command is scheduled.
 	@Override
 	public void execute() {
-		if (!useLimelights) {
-			targetHeading = lockedOnHeading - drivetrain.getPigeon2().getAngle();
+		if (useLimelights) {
 
-			SwerveRequest.FieldCentric pointAtTag = new SwerveRequest.FieldCentric();
-			pidOutput = headingController.calculate(0, targetHeading);
-			drivetrain.setControl(pointAtTag.withRotationalRate(-pidOutput));
+			targetHeading = limelight.getTargetX();
 
 		} else {
-			for (Limelight limelight : Limelight.filterLimelights(limelights)) {
-				targetHeading = limelight.getTargetX();
-			}
 
-			SwerveRequest.RobotCentric pointAtTag = new SwerveRequest.RobotCentric();
-			pidOutput = headingController.calculate(0, targetHeading);
-			drivetrain.setControl(pointAtTag.withRotationalRate(-pidOutput));
+			lockedOnHeading = LightningShuffleboard.getDouble("PointAtTag", "LockOnHeading", 0);
+			LightningShuffleboard.setDouble("PointAtTag", "Drivetrain Angle", drivetrain.getPigeon2().getAngle());
+			targetHeading = lockedOnHeading - drivetrain.getPigeon2().getAngle();
+
 		}
 
 		lockedOnHeading = LightningShuffleboard.getDouble("PointAtTag", "LockOnHeading", 0);
@@ -108,8 +112,6 @@ public void execute() {
 	@Override
 	public void end(boolean interrupted) {
 		limelight.setPipeline(limelightId);
-		//Do we want
-		drivetrain.applyRequest(() -> brake); // TODO test if this applies brake
 	}
 
 	// Returns true when the command should end.

src/main/java/frc/robot/subsystems/Collision.java

@@ -1,36 +0,0 @@
-// 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 frc.robot.subsystems;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants.VisionConstants;
-import frc.thunder.shuffleboard.LightningShuffleboard;
-
-public class Collision extends SubsystemBase {
-  private Swerve drivetrain;
-  public double pitch;
-  public double roll;
-
-  public Collision(Swerve drivetrain) {
-    this.drivetrain = drivetrain;
-    // initiallizes pitch and roll (so we don't print null values)
-    pitch = drivetrain.getPigeon2().getPitch().getValueAsDouble();
-    roll = drivetrain.getPigeon2().getRoll().getValueAsDouble();
-
-    // displays whether the robot is off balance
-    LightningShuffleboard.setBoolSupplier("Collision", "offBalance", () -> (Math.abs(pitch) > VisionConstants.COLLISION_DEADZONE || Math.abs(roll) > VisionConstants.COLLISION_DEADZONE));
-    LightningShuffleboard.setDoubleSupplier("Collision", "pitch", () -> pitch);
-    LightningShuffleboard.setDoubleSupplier("Collision", "roll", () -> roll);
-
-  }
-
-  @Override
-  public void periodic() {
-    // Updates roll and pitch values
-    pitch = drivetrain.getPigeon2().getPitch().getValueAsDouble();
-    roll = drivetrain.getPigeon2().getRoll().getValueAsDouble();
-  }
-}
-*/