best option of minus signs

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
index af107681d2e209680b9c9670dea8224450d00ec2..04b5b59df370d33b091afa9d6e5b3e026cf678f6 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
@@ -28,102 +28,18 @@ import frc.robot.util.TrenchAssist.TrenchAssistConstants;
 
 public class TrenchAssist2 {
 
-    // public void execute() {
-    //     trenchAlign = SmartDashboard.getBoolean("trench aligning", trenchAlign);
-    //     trenchAssist = SmartDashboard.getBoolean("trench aligning", trenchAssist);
-
-    //     SmartDashboard.putBoolean("trench aligning", trenchAlign);
-    //     SmartDashboard.putBoolean("trench assisting", trenchAssist);
-
-    //     double forwardTranslation = driver.getForwardTranslation();
-    //     double sideTranslation = driver.getSideTranslation();
-    //     double rotation = -driver.getRotation();
-
-    //     double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1;
-
-    //     forwardTranslation *= slowFactor;
-    //     sideTranslation *= slowFactor;
-    //     rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1;
-
-    //     int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1;
-    //     forwardTranslation *= allianceReversal;
-    //     sideTranslation *= allianceReversal;
-
-    //     ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
-    //     ChassisSpeeds corrected = DriverAssist.calculate(drive, driverInput, drive.getDesiredPose(), true);
-
-    //     if (trenchAlign) {
-    //         for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) {
-    //             if (rectangle.contains(drive.getPose().getTranslation())) {
-    //                 drive.setIsAlign(true);
-
-    //                 if (drive.getPose().getRotation().getDegrees() > 90
-    //                         && drive.getPose().getRotation().getDegrees() < 270) {
-    //                     drive.setAlignAngle(0.0);
-    //                 } else if (drive.getPose().getRotation().getDegrees() > 270
-    //                         && drive.getPose().getRotation().getDegrees() < 90) {
-    //                     drive.setAlignAngle(Units.degreesToRadians(180));
-    //                 }
-    //             } else {
-    //                 drive.setIsAlign(false);
-    //             }
-    //         }
-    //     }
-
-    //     if (trenchAssist) {
-    //         Translation2d calculated = calculateCorrection(TrenchAssistConstants.OBSTACLES);
-    //         ChassisSpeeds assisted = new ChassisSpeeds(corrected.vxMetersPerSecond + calculated.getX(),
-    //                 corrected.vyMetersPerSecond + calculated.getY(), corrected.omegaRadiansPerSecond);
-
-    //         drive(assisted);
-    //     } else {
-    //         drive(corrected);
-    //     }
-
-    // }
-
-    /**
-     * 
-     * @param rectangle the rectangle that the ray should check against
-     * @param point     the origin point of the ray
-     * @param velocity  vector of the ray, magnitude is speed
-     * @param time      how far into the future to check
-     * @return returns Optional full if intersects rectangle within time param into
-     *         future, time/100
-     *         second resolution for raymarching, returns distance that it hits it
-     *         at
-     */
-    public static Optional<Double> rayCast(Rectangle2d rectangle, Translation2d point, Translation2d velocity,
-            double time) {
-        // double distance = velocity.getNorm();
-        // Translation2d normalized = new Translation2d(velocity.getX() / distance,
-        // velocity.getY() / distance);
-
-        for (int i = 0; i <= 100; i++) {
-            double t = (i * time) / 100.0; // seconds into the future
-            Translation2d ray = velocity.times(t).plus(point);
-            if (rectangle.contains(ray)) {
-                return Optional.of(t);
-            }
-        }
-
-        return Optional.empty();
-    }
-
-
-
-    public static ChassisSpeeds funky(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
-        ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, 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
         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().unaryMinus());
+        var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw());
         return new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond);
     }
 
-    public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
+    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);

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
index 29654bbc88eb7c71d1181c7ed16786c87fef72cd..93bf2f69062ddbf81b88e787193c0ecaebb83806 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
@@ -17,7 +17,7 @@ public class TrenchAssistConstants {
         new Rectangle2d(new Translation2d(4.03 - .5, 8.07), new Translation2d(5.22 + .5, 8.07 - 3)),
         new Rectangle2d(new Translation2d(11.32 - .5, 0), new Translation2d(12.51 + .5, 3)),
         new Rectangle2d(new Translation2d(11.32 - .5, 8.07), new Translation2d(12.51 + .5, 8.07 - 3)),
-        // new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)),
+        new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)),
     };
 
     public static final double[] SLIDE_ZONES = new double[]{