Fix

Mechanical-Advantage · Jun 13, 2024 · b28c0d3 · b28c0d3
1 parent 5f7b240
commit b28c0d3
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 22 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -336,10 +336,6 @@ public void setDemoTagPose(Pose3d demoTagPose) {
 
   private static final LoggedTunableNumber demoTargetDistance =
       new LoggedTunableNumber("RobotState/DemoTargetDistance", 2.0);
-  private static final Translation3d upDirection = new Translation3d(0.0, 0.0, 1.0);
-  private static final Translation3d leftDirection = new Translation3d(0.0, 1.0, 0.0);
-  private static final Translation3d downDirection = new Translation3d(0.0, 0.0, -1.0);
-  private static final Translation3d rightDirection = new Translation3d(0.0, -1.0, 0.0);
 
   public Optional<DemoAimingParameters> getDemoTagParameters() {
     if (latestDemoParamters != null) {
@@ -350,25 +346,12 @@ public Optional<DemoAimingParameters> getDemoTagParameters() {
     if (demoTagPose == null) return Optional.empty();
 
     // Calculate target pose.
-    // Determine tag rotation
-    double maxZ = 0.0;
-    int maxIndex = 0;
-    int index = 0;
-    for (var tagDirection :
-        new Translation3d[] {upDirection, leftDirection, downDirection, rightDirection}) {
-      double z = demoTagPose.transformBy(new Transform3d(tagDirection, new Rotation3d())).getZ();
-      if (z > maxZ) {
-        maxZ = z;
-        maxIndex = index;
-      }
-      index++;
-    }
-    Rotation2d robotRotation = new Rotation2d(Math.PI + Math.PI / 2.0 * maxIndex);
     Pose2d targetPose =
         demoTagPose
             .toPose2d()
             .transformBy(
-                new Transform2d(new Translation2d(demoTargetDistance.get(), 0.0), robotRotation));
+                new Transform2d(
+                    new Translation2d(demoTargetDistance.get(), 0.0), new Rotation2d(Math.PI)));
 
     // Calculate heading without movement.
     Translation2d demoTagFixed = demoTagPose.getTranslation().toTranslation2d();

diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java b/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java
@@ -61,7 +61,7 @@ private Command lookAtTag() {
                             .map(RobotState.DemoAimingParameters::targetHeading)
                             .orElseGet(
                                 () -> RobotState.getInstance().getEstimatedPose().getRotation())),
-            drive::clearAutoAlignGoal)
+            drive::clearHeadingGoal)
         .alongWith(
             superstructure.setGoalWithConstraintsCommand(
                 Superstructure.Goal.AIM_AT_DEMO_TAG, Arm.smoothProfileConstraints.get()));

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java
@@ -384,8 +384,10 @@ public void setAutoAlignGoal(
       Supplier<Pose2d> poseSupplier,
       Supplier<Translation2d> feedforwardSupplier,
       boolean slowMode) {
-    currentDriveMode = DriveMode.AUTO_ALIGN;
-    autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode);
+    if (DriverStation.isEnabled()) {
+      currentDriveMode = DriveMode.AUTO_ALIGN;
+      autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode);
+    }
   }
 
   /** Clears the current auto align goal. */