tag align tweaks

team20 · Feb 8, 2025 · 4913c84 · 4913c84
1 parent 581687c
commit 4913c84
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -44,7 +44,8 @@ public void bindDriveControls() {
 						() -> -m_driverController.getLeftX(),
 						() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(),
 						m_driverController.getHID()::getSquareButton,
-						m_driverController.button(1)));
+						m_driverController.getHID()::getL1Button));
+		// m_driverController.button(1)));
 	}
 
 	@Override

diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -250,15 +250,16 @@ public Command driveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSp
 			if (isAlign.getAsBoolean()) {
 
 				// Calculate transform (x,y,theta) of driver movement
-				final Transform2d jsTransform = (fwdSpeed == 0 && strSpeed == 0)
-						? new Transform2d(0, 0, Rotation2d.kZero)
-						: new Transform2d(fwdSpeed, strSpeed, new Rotation2d(fwdSpeed, strSpeed));
+				final Transform2d jsTransform = new Transform2d(fwdSpeed, strSpeed, new Rotation2d(fwdSpeed, strSpeed));
 
+				Translation2d stillVectah = new Translation2d(1, getPose().getRotation());
 				// Use the target that the driver is heading towards
 				AprilTag target = m_vision.getBestTarget(
-						(fwdSpeed == 0 && strSpeed == 0) ? new Transform2d(1, 0, Rotation2d.kZero)
-								: jsTransform);
+						(fwdSpeed == 0 && strSpeed == 0) ? stillVectah.toVector()
+								: jsTransform.getTranslation().toVector());
 				Transform3d targetPos = m_vision.getTransformToTarget(target);
+				SmartDashboard.putNumber("still VECTAH X", stillVectah.toVector().get(0));
+				SmartDashboard.putNumber("still VECTAH Y", stillVectah.toVector().get(1));
 
 				// Calculate the transform towards the chosen target and find the differences in
 				// position and angle between the driver's movement and the tag.

diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
@@ -17,7 +17,6 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N2;
@@ -102,7 +101,7 @@ public double unitDot(Vector<N2> v1, Vector<N2> v2) {
 	 * @param joysticks The {@Translation2d} of the joysticks' movement.
 	 * @return The {@AprilTag} target that the driver is moving towards.
 	 */
-	public AprilTag getBestTarget(Transform2d joysticks) {
+	public AprilTag getBestTarget(Vector joysticks) {
 		// List<AprilTag> tags = kFieldLayout.getTags();
 		// tags.sort(
 		// Comparator.comparing(
@@ -116,7 +115,7 @@ public AprilTag getBestTarget(Transform2d joysticks) {
 				.map(
 						t -> Map.entry(
 								t, (1 - unitDot(
-										joysticks.getTranslation().toVector(),
+										joysticks,
 										getTransformToTarget(t).getTranslation().toTranslation2d().toVector()))))
 				.reduce((target1, target2) -> target1.getValue() < target2.getValue() ? target1 : target2).get()
 				.getKey();