From: moo <moogoesmeow123@gmail.com>
Date: Fri, 6 Mar 2026 23:05:23 +0000 (-0800)
Subject: yay fix
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=eb9c2f7204403cca7c91ca8c58e5069b2803ffb6;p=FRC2026.git

yay fix
---

diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
index c481e0d..4c31496 100644
--- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Rectangle2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Robot;
 import frc.robot.constants.swerve.DriveConstants;
@@ -40,6 +41,8 @@ public class DefaultDriveCommand extends Command {
         trenchAssistPid.setIZone(2);
         trenchAssistPid.setIntegratorRange(-1, 1);
 
+        SmartDashboard.putNumber("0 degrees snap location", 0);
+        SmartDashboard.putNumber("", 0);
     }
 
     @Override
@@ -76,8 +79,17 @@ public class DefaultDriveCommand extends Command {
                 swerve.setIsAlign(true);
 
                 double yawDegrees = swerve.getYaw().getDegrees();
-                double snappedDeg = Math.round(yawDegrees / 90.0) * 90.0;
-                swerve.setAlignAngle(Units.degreesToRadians(snappedDeg));
+                Logger.recordOutput("yawdegs", yawDegrees);
+                // double snappedDeg = Math.round(yawDegrees / 90.0) * 90.0;
+                if (Math.abs(yawDegrees) <= 90) {
+                    Logger.recordOutput("0degs", false);
+                    swerve.setAlignAngle(Units.degreesToRadians(0.0));
+                } else {
+                    swerve.setAlignAngle(Units.degreesToRadians(180.0));
+                    Logger.recordOutput("0degs", true);
+                }
+                // swerve.setAlignAngle(snappedDeg);
+                // Logger.recordOutput("snappy", snappedDeg);
             } else {
                 Logger.recordOutput("InAlignZone", false);
                 swerve.setIsAlign(false);
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 21afaf3..7a8b54b 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -243,7 +243,7 @@ public class Drivetrain extends SubsystemBase {
      */
     public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
         double rot = rotationController.calculate(getYaw().getRadians(), heading);
-        ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
+        ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, -rot);
         if (fieldRelative) {
             speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
         }
diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
index 29a3284..3cd413b 100644
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
@@ -17,7 +17,7 @@ public class TrenchAssistConstants {
             new Rectangle2d(new Translation2d(4.03 - .5, 8.07), new Translation2d(5.22 + .5, 8.07 - 3)),
             new Rectangle2d(new Translation2d(11.32 - .5, 0), new Translation2d(12.51 + .5, 3)),
             new Rectangle2d(new Translation2d(11.32 - .5, 8.07), new Translation2d(12.51 + .5, 8.07 - 3)),
-            new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)),
+            new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(-99.0, -99.0)),
     };
 
     public static final double[] SLIDE_LATITUDES = new double[]{