From 55c74e2e925b4e7b6198e4ba0bfae883f95ec3ea Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 29 Feb 2024 19:15:08 -0800
Subject: [PATCH] Auto position testing

---
 src/main/java/frc/robot/Constants.java            |  2 +-
 src/main/java/frc/robot/RobotContainer.java       |  8 ++++++--
 .../java/frc/robot/commands/AutoPosition.java     | 15 +++++++--------
 3 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index a66ece2..588dccf 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -261,7 +261,7 @@ public static class AutoConstants {
 
 	public static class VisionConstants {
 
-		public static final Transform3d CAMERA_POSE = new Transform3d(0.5, 0, 0.25, new Rotation3d());
+		public static final Transform3d CAMERA_POSE = new Transform3d(0, 0, 0, new Rotation3d());
 		public static final String CAMERA_NAME = "Arducam_OV9281_USB_Camera";
 	}
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ddd26d2..979b3d3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -30,6 +30,7 @@
 import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
@@ -163,12 +164,15 @@ public void setUpDriveController() {
 			// xbox.povLeft().whileTrue(Commands.run(drivetrain::toDefaultStates,
 			// drivetrain));
 
-			xbox.b().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel));
+			// xbox.b().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel));
 			xbox.povDown().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel));
 
 			xbox.povUp().onTrue(Commands.runOnce(inputs::increaseSpeedLevel));
 
-			xbox.a().whileTrue(new AutoPosition(drivetrain, vision));
+			var autoPosition = new AutoPosition(drivetrain, vision);
+			xbox.a().onTrue(autoPosition);
+			// xbox.a().onTrue(autoPosition);
+			xbox.b().onTrue(new InstantCommand(() -> drivetrain.getCurrentCommand().cancel(), drivetrain));
 
 			drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs));
 		}
diff --git a/src/main/java/frc/robot/commands/AutoPosition.java b/src/main/java/frc/robot/commands/AutoPosition.java
index 2bd01e1..1ddc99c 100644
--- a/src/main/java/frc/robot/commands/AutoPosition.java
+++ b/src/main/java/frc/robot/commands/AutoPosition.java
@@ -58,17 +58,16 @@ public void initialize() {
 		if (dist3d == null) {
 			return;
 		}
+
 		double angle = dist3d.getRotation().getZ();
-		Translation2d trans = new Translation2d(dist3d.getY() - AutoConstants.PREFERRED_TAG_DISTANCE * Math.cos(angle),
-				dist3d.getX() - AutoConstants.PREFERRED_TAG_DISTANCE * Math.sin(angle));
+		Translation2d trans = new Translation2d(
+				dist3d.getX() - 0.5, dist3d.getY());
 		SmartDashboard.putNumber("ANGLE", angle);
-		SmartDashboard.putNumber("Y", trans.getY());
-		SmartDashboard.putNumber("X", trans.getX());
-		Rotation2d rot = new Rotation2d(-(Math.PI-angle));
+		SmartDashboard.putNumber("tr X", trans.getX());
+		SmartDashboard.putNumber("tr Y", trans.getY());
+		Rotation2d rot = new Rotation2d(-(Math.PI - angle));
 		Commands.sequence(
-				new AutoRotateTo(drivetrain, rot, false)
-				,new AutoDriveTo(drivetrain, trans)
-				).schedule();
+				new AutoRotateTo(drivetrain, rot, false), new AutoDriveTo(drivetrain, trans)).schedule();
 		;
 
 	}