cleanup stuff

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
index 4d2a3e3dc6985dbd706d1bc9e7d9cb9753f677b2..259e6a3a75509cfb87d92b7997970969a5618eb7 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
@@ -20,34 +20,33 @@ public class TrenchAssist {
 
         double distanceFromSlideLatitude;
 
+        //pick which side of field to go on
         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);
 
+        //deadzone
         if (Math.abs(distanceFromSlideLatitude) < Units.inchesToMeters(3)) {
             correctionVelocity = 0.0;
         }
 
+        // set y speed to pid calculation
         ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, correctionVelocity,
                 chassisSpeeds.omegaRadiansPerSecond);
 
-        var y = new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond)
+        //rotate once
+        Translation2d y = new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond)
                 .rotateBy(drive.getYaw());
 
-        var z = ChassisSpeeds.fromFieldRelativeSpeeds(
+        //rotate twice
+        return ChassisSpeeds.fromFieldRelativeSpeeds(
                 new ChassisSpeeds(y.getX(), y.getY(), horizontalSpeeds.omegaRadiansPerSecond),
                 drive.getYaw());
-
-        return z;
-
     }
 
     public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive) {