From: moo <moogoesmeow123@gmail.com>
Date: Sun, 22 Feb 2026 19:47:49 +0000 (-0800)
Subject: HOLY SHIT IT WOKRS
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=71c9e21be815f824bbf867c5fcdd2099a576e8f3;p=FRC2026.git

HOLY SHIT IT WOKRS

this is bullshit
makes zero frickin sense
---

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
index 04b5b59..ca15a9f 100644
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
@@ -28,7 +28,7 @@ import frc.robot.util.TrenchAssist.TrenchAssistConstants;
 
 public class TrenchAssist2 {
 
-    public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
+    public static ChassisSpeeds attempt2(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
         ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, drive.getYaw().unaryMinus()); //this singular unary minus is best we can get
         ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(speedsFieldRelative.vxMetersPerSecond, 0.0, speedsFieldRelative.omegaRadiansPerSecond);
 
@@ -39,21 +39,53 @@ public class TrenchAssist2 {
         return new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond);
     }
 
-    public static ChassisSpeeds funky(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
-        double yawRadians = drive.getYaw().unaryMinus().getRadians();
+    public static ChassisSpeeds attempt3(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
+        ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, drive.getYaw().unaryMinus()); //this singular unary minus is best we can get
+        ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(speedsFieldRelative.vxMetersPerSecond, 0.0, speedsFieldRelative.omegaRadiansPerSecond);
+
+        Logger.recordOutput("slideTranslation", new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond));
+
+        var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond);
+        var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw());
+
+        var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond), drive.getYaw());
+
+        return z;
+    }
+
+    public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
+        //ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, drive.getYaw().unaryMinus()); //this singular unary minus is best we can get
+        ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, 0.0, chassisSpeeds.omegaRadiansPerSecond);
+
+        Logger.recordOutput("slideTranslation", new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond));
+
+        var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond);
+        var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw());
+
+        var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond), drive.getYaw());
+
+        return z;
+
+    }
+
+
+
+
+    // public static ChassisSpeeds funky(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
+    //     double yawRadians = drive.getYaw().unaryMinus().getRadians();
         
-        double fieldRelativeX = chassisSpeeds.vxMetersPerSecond * Math.cos(yawRadians) - chassisSpeeds.vyMetersPerSecond * Math.sin(yawRadians);
-        double fieldRelativeY = chassisSpeeds.vxMetersPerSecond * Math.sin(yawRadians) + chassisSpeeds.vyMetersPerSecond * Math.cos(yawRadians);
+    //     double fieldRelativeX = chassisSpeeds.vxMetersPerSecond * Math.cos(yawRadians) - chassisSpeeds.vyMetersPerSecond * Math.sin(yawRadians);
+    //     double fieldRelativeY = chassisSpeeds.vxMetersPerSecond * Math.sin(yawRadians) + chassisSpeeds.vyMetersPerSecond * Math.cos(yawRadians);
         
-        fieldRelativeY = 0;
+    //     fieldRelativeY = 0;
         
-        double neutralizedRobotX = fieldRelativeX * Math.cos(yawRadians) + fieldRelativeY * Math.sin(yawRadians);
-        double neutralizedRobotY = -fieldRelativeX * Math.sin(yawRadians) + fieldRelativeY * Math.cos(yawRadians);
+    //     double neutralizedRobotX = fieldRelativeX * Math.cos(yawRadians) + fieldRelativeY * Math.sin(yawRadians);
+    //     double neutralizedRobotY = -fieldRelativeX * Math.sin(yawRadians) + fieldRelativeY * Math.cos(yawRadians);
 
-        var x = new Translation2d(neutralizedRobotX, neutralizedRobotY).rotateBy(drive.getYaw().unaryMinus());
+    //     var x = new Translation2d(neutralizedRobotX, neutralizedRobotY).rotateBy(drive.getYaw().unaryMinus());
 
-        return new ChassisSpeeds(x.getX(), x.getY(), chassisSpeeds.omegaRadiansPerSecond);
-    }
+    //     return new ChassisSpeeds(x.getX(), x.getY(), chassisSpeeds.omegaRadiansPerSecond);
+    // }
 
     public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive){
         Rotation2d yaw = drive.getYaw();
diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
index 93bf2f6..5aa96ef 100644
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
@@ -20,7 +20,7 @@ public class TrenchAssistConstants {
         new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)),
     };
 
-    public static final double[] SLIDE_ZONES = new double[]{
+    public static final double[] SLIDE_LATITUDES = new double[]{
         8.07 - Units.inchesToMeters(25.0),
         Units.inchesToMeters(25.0),
     };