added dot product tagDrive

team20 · Jan 20, 2025 · 43ed236 · 43ed236
1 parent 5071639
commit 43ed236
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -7,9 +7,11 @@
 import static edu.wpi.first.units.Units.*;
 import static frc.robot.Constants.DriveConstants.*;
 
+import java.util.Map;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 import com.studica.frc.AHRS;
@@ -251,11 +253,22 @@ public Command driveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSp
 			fwdSpeed = Math.signum(fwdSpeed) * (fwdSpeed * fwdSpeed);
 			strSpeed = Math.signum(strSpeed) * (strSpeed * strSpeed);
 
-			if (isAlign.getAsBoolean() && m_vision.getLatestResult().hasTargets()) {
+			PhotonPipelineResult result = m_vision.getLatestResult();
+			if (isAlign.getAsBoolean() && result.hasTargets()) {
+
 				// Detect nearest April Tag and get translation data from joysticks and tag
 				// Calculate translation between driver movement and target movement to tag
-				PhotonTrackedTarget target = m_vision.getBestTarget(m_vision.getLatestResult());
-				Translation2d jsTranslation = new Translation2d(forwardSpeed.getAsDouble(), strafeSpeed.getAsDouble());
+				final Translation2d jsTranslation = new Translation2d(forwardSpeed.getAsDouble(),
+						strafeSpeed.getAsDouble());
+				PhotonTrackedTarget target = result.getTargets().stream()
+						.map(
+								t -> Map.entry(
+										t, (1 - m_vision.unitDot(
+												jsTranslation.toVector(),
+												m_vision.getTranslationToTarget(t).toVector()))))
+						.reduce((e1, e2) -> e1.getValue() < e2.getValue() ? e1 : e2).get().getKey();
+				// target = m_vision.getBestTarget(m_vision.getLatestResult());
+
 				Translation2d targetPos = m_vision.getTranslationToTarget(target);
 				Translation2d posDiff = jsTranslation.minus(targetPos);
 

diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
@@ -20,6 +20,7 @@
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -77,6 +78,10 @@ public PhotonPipelineResult getLatestResult() {
 		return m_camera.getLatestResult();
 	}
 
+	public double unitDot(Vector v1, Vector v2) {
+		return v1.unit().dot(v2.unit());
+	}
+
 	public PhotonTrackedTarget getBestTarget(PhotonPipelineResult r) {
 		return r.getBestTarget();
 	}