Works, but slide latitudes are wrong

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
index ca15a9f738fcc0ead641e9dc1f61ac6115f78451..0a2f1425deff376d7e1e90688cc8af7b4b97f26a 100644 (file)
--- 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 5aa96ef78b405afbf2a008aed99dd8ec079f5602..1c9c5cacec5f7be4965c586d287e877545e6446b 100644 (file)
--- 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