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 04b5b59df370d33b091afa9d6e5b3e026cf678f6..ca15a9f738fcc0ead641e9dc1f61ac6115f78451 100644 (file)
--- 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 93bf2f69062ddbf81b88e787193c0ecaebb83806..5aa96ef78b405afbf2a008aed99dd8ec079f5602 100644 (file)
--- 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),
     };