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 c481e0d87f3d0746c3b9668fa5caaf5d15b1e033..4c314961081bf9857a181e34d97edc089dffab4d 100644 (file)
--- 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 21afaf3eb7f9ad09bd3a72491e9d84a0c8a18564..7a8b54bb3118dffcce8f5e9f2a69956033b61465 100644 (file)
--- 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 29a32847636e326a32b4da821cb9a54541111f51..3cd413b9fbd148fd7abdf8487600d6708457d840 100644 (file)
--- 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[]{