rename

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
new file mode 100644 (file)
index 0000000..47a29ed
--- /dev/null
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
@@ -0,0 +1,66 @@
+package frc.robot.util.TrenchAssist;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+public class TrenchAssist {
+
+    public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds, PIDController pid) {
+        // ChassisSpeeds speedsFieldRelative =
+        // ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds,
+        // drive.getYaw().unaryMinus());
+
+        double distanceFromSlideLatitude;
+
+        if (drive.getPose().getY() > (FieldConstants.FIELD_WIDTH / 2.0)) {
+            distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[0]);
+        } else {
+            distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[1]);
+        }
+
+        Logger.recordOutput("slides",
+                new Pose2d[] { new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[0], Rotation2d.kZero),
+                        new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[1], Rotation2d.kZero) });
+
+        double correctionVelocity = pid.calculate(distanceFromSlideLatitude, 0);
+
+        if (distanceFromSlideLatitude < Units.inchesToMeters(3)) {
+            correctionVelocity = 0.0;
+        }
+
+        ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, correctionVelocity,
+                chassisSpeeds.omegaRadiansPerSecond);
+
+        var y = new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond)
+                .rotateBy(drive.getYaw());
+
+        var z = ChassisSpeeds.fromFieldRelativeSpeeds(
+                new ChassisSpeeds(y.getX(), y.getY(), horizontalSpeeds.omegaRadiansPerSecond),
+                drive.getYaw());
+
+        return z;
+
+    }
+
+    public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive) {
+        Rotation2d yaw = drive.getYaw();
+        Translation2d robotRelative = translation.rotateBy(yaw);
+        return new ChassisSpeeds(robotRelative.getX(), robotRelative.getY(), 0.0);
+
+    }
+
+    public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive) {
+        Rotation2d yaw = drive.getYaw();
+        Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+        return robotTranslation.rotateBy(yaw.times(-1));
+
+    }
+}

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
deleted file mode 100644 (file)
index cd93f0d..0000000
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
+++ /dev/null
@@ -1,66 +0,0 @@
-package frc.robot.util.TrenchAssist;
-
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-
-public class TrenchAssist2 {
-
-    public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds, PIDController pid) {
-        // ChassisSpeeds speedsFieldRelative =
-        // ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds,
-        // drive.getYaw().unaryMinus());
-
-        double distanceFromSlideLatitude;
-
-        if (drive.getPose().getY() > (FieldConstants.FIELD_WIDTH / 2.0)) {
-            distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[0]);
-        } else {
-            distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[1]);
-        }
-
-        Logger.recordOutput("slides",
-                new Pose2d[] { new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[0], Rotation2d.kZero),
-                        new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[1], Rotation2d.kZero) });
-
-        double correctionVelocity = pid.calculate(distanceFromSlideLatitude, 0);
-
-        if (distanceFromSlideLatitude < Units.inchesToMeters(3)) {
-            correctionVelocity = 0.0;
-        }
-
-        ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, correctionVelocity,
-                chassisSpeeds.omegaRadiansPerSecond);
-
-        var y = new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond)
-                .rotateBy(drive.getYaw());
-
-        var z = ChassisSpeeds.fromFieldRelativeSpeeds(
-                new ChassisSpeeds(y.getX(), y.getY(), horizontalSpeeds.omegaRadiansPerSecond),
-                drive.getYaw());
-
-        return z;
-
-    }
-
-    public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive) {
-        Rotation2d yaw = drive.getYaw();
-        Translation2d robotRelative = translation.rotateBy(yaw);
-        return new ChassisSpeeds(robotRelative.getX(), robotRelative.getY(), 0.0);
-
-    }
-
-    public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive) {
-        Rotation2d yaw = drive.getYaw();
-        Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
-        return robotTranslation.rotateBy(yaw.times(-1));
-
-    }
-}