Auto position testing

redshiftrobotics · Mar 1, 2024 · 55c74e2 · 55c74e2
1 parent c505222
commit 55c74e2
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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();
 		;
 
 	}