PR for vision branch - working note detection and new drive method.

src/main/java/frc/team2412/robot/Controls.java

@@ -141,7 +141,7 @@ private void bindDrivebaseControls() {
 	// leaving this code in in case we need to test outside of auto
 	/*
 	public void bindLimelightControls() {
-		getWithinDistanceTrigger.onTrue(
+		getWithinDistanceTrigger.whileTrue(
 				new DriveToNoteCommand(s.drivebaseSubsystem, s.limelightSubsystem));
 	}
 	*/

src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java

@@ -24,19 +24,21 @@ public DriveToNoteCommand(
 
 	@Override
 	public void execute() {
-		Translation2d move =
-				new Translation2d(
-						INVERT_DRIVE_DIRECTION
-								* Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()),
-						0.0);
-		Rotation2d turn =
-				new Rotation2d()
-						.fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset());
-		drivebaseSubsystem.drive(move, turn, false);
+		if (limelightSubsystem.hasTargets()) {
+			Translation2d move =
+					new Translation2d(
+							INVERT_DRIVE_DIRECTION
+									* Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()),
+							0.0);
+			Rotation2d turn =
+					new Rotation2d()
+							.fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset());
+			drivebaseSubsystem.drive(move, turn, false);
+		}
 	}
 
 	@Override
 	public boolean isFinished() {
-		return (limelightSubsystem.isWithinDistance());
+		return (limelightSubsystem.isWithinDistance() || !limelightSubsystem.hasTargets());
 	}
 }

src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java

@@ -1,9 +1,5 @@
 package frc.team2412.robot.subsystems;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -23,7 +19,6 @@ public class LimelightSubsystem extends SubsystemBase {
 	final NetworkTable networkTable;
 
 	String currentPoseString;
-	String targetPoseString;
 
 	// network tables
 
@@ -34,7 +29,6 @@ public LimelightSubsystem() {
 
 		// logging
 		currentPoseString = "";
-		targetPoseString = "";
 
 		networkTable = NetworkTableInstance.getDefault().getTable("limelight");
 		ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight");
@@ -59,10 +53,6 @@ public LimelightSubsystem() {
 				.addString("Current Pose ", this::getCurrentPoseString)
 				.withPosition(0, 1)
 				.withSize(4, 1);
-		limelightTab
-				.addString("Target Pose ", this::getTargetPoseString)
-				.withPosition(0, 2)
-				.withSize(4, 1);
 		GOAL_DISTANCE_FROM_NOTE =
 				limelightTab
 						.addPersistent("Goal distance", 20.0)
@@ -102,41 +92,17 @@ public double getDistanceFromTargetInches() {
 
 		// distance = (W x F) / P
 		// returns inches for testing purposes, will divide by 39.3700787 to return meters
-		return (14 * focal_length) / getBoxWidth();
-	}
-
-	// tan(degree) * distance = sideways distance
-
-	// target height / tan(vertical angle)
-
-	public Pose2d getTargetPose(Pose2d currentPose) {
-
-		// math thing to get target pose using current pose
-
-		Rotation2d targetHeading =
-				new Rotation2d(
-						currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset()));
-
-		double targetDistance = Units.inchesToMeters(getDistanceFromTargetInches());
-
-		Translation2d translationOffset = new Translation2d(targetDistance, targetHeading);
-		Pose2d targetPose =
-				new Pose2d(currentPose.getTranslation().plus(translationOffset), targetHeading);
-
-		currentPoseString = currentPose.toString();
-		targetPoseString = targetPose.toString();
-
-		return targetPose;
+		if (hasTargets()) {
+			return (14 * focal_length) / getBoxWidth();
+		} else {
+			return 0.0;
+		}
 	}
 
 	public String getCurrentPoseString() {
 		return currentPoseString;
 	}
 
-	public String getTargetPoseString() {
-		return targetPoseString;
-	}
-
 	public boolean isWithinDistance() {
 		return (getDistanceFromTargetInches() <= GOAL_DISTANCE_FROM_NOTE.getDouble(20.0));
 	}