From 90d7266b70db6cc05fd88a7a6d8642e0cf420a76 Mon Sep 17 00:00:00 2001 From: moo Date: Sun, 22 Feb 2026 12:34:34 -0800 Subject: [PATCH] Works, but slide latitudes are wrong --- .../util/TrenchAssist/TrenchAssist2.java | 55 ++++--------------- .../TrenchAssist/TrenchAssistConstants.java | 7 ++- 2 files changed, 16 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java index ca15a9f..0a2f142 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java @@ -28,36 +28,22 @@ import frc.robot.util.TrenchAssist.TrenchAssistConstants; public class TrenchAssist2 { - 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); + private static final double SCALE = 1.0; - 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()); - return new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond); - } - - 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()); + 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 - var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond), drive.getYaw()); + double distanceFromSlideLatitude; - return z; - } + if (drive.getPose().getY() > (8.07 / 2.0)){ + distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[0]); + } else { + distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[1]); + } - 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); + var impulse = -distanceFromSlideLatitude * SCALE; - Logger.recordOutput("slideTranslation", new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond)); + ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, impulse, chassisSpeeds.omegaRadiansPerSecond); var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond); var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw()); @@ -68,25 +54,6 @@ public class TrenchAssist2 { } - - - - // 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); - - // fieldRelativeY = 0; - - // 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()); - - // return new ChassisSpeeds(x.getX(), x.getY(), chassisSpeeds.omegaRadiansPerSecond); - // } - public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive){ Rotation2d yaw = drive.getYaw(); Translation2d robotRelative = translation.rotateBy(yaw); diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java index 5aa96ef..1c9c5ca 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java @@ -21,8 +21,11 @@ public class TrenchAssistConstants { }; public static final double[] SLIDE_LATITUDES = new double[]{ - 8.07 - Units.inchesToMeters(25.0), - Units.inchesToMeters(25.0), + // 8.07 - Units.inchesToMeters(30.0), + // Units.inchesToMeters(30.0), should be accurate, i think our field is slightly too small + 6.550, + 0.668, + }; } \ No newline at end of file -- 2.39.5