shuttling tuned (seperate non dynamic TOF for shuttling)

still needs last interpolation key

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 27fb45b4573f6ea9884e6613e127e84564a51485..d767fe857db237fbb7aa06ef651daa08ec65fc9e 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -66,6 +66,7 @@ public class Superstructure extends Command {
     private double turretOffset = 0.0;
 
     private double distanceFromTarget = 0.0;
+    private double shuttlingTOFMultiplier = 0.8;
 
     // private double TOFAdjustment = 0.85;
     // private double TOFAdjustment = 1.1;
@@ -111,11 +112,12 @@ public class Superstructure extends Command {
          * So we make a bunch of guesses until it converges
          * Early exit when change < 1mm to avoid unnecessary iterations
          */
+        boolean shuttling = target.equals(FieldConstants.getHubTranslation().toTranslation2d());
         for (int i = 0; i < 20; i++) {
             Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
             
             Translation3d target3d = new Translation3d(target.getX(), target.getY(),
-                target.equals(FieldConstants.getHubTranslation().toTranslation2d()) ?
+                shuttling ?
                 FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
 
             goalState = ShooterPhysics.getShotParams(
@@ -123,7 +125,13 @@ public class Superstructure extends Command {
             target3d.minus(lookahead3d),
             2.0);
 
-            timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get();
+            if (!shuttling) {
+                timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get();
+            } else {
+                double distance = target.getDistance(lookaheadPose.getTranslation());
+                timeOfFlight = distance * shuttlingTOFMultiplier;
+            }
+
             double offsetX = turretVelocityX * timeOfFlight;
             double offsetY = turretVelocityY * timeOfFlight;
             Pose2d newLookaheadPose =
@@ -251,6 +259,9 @@ public class Superstructure extends Command {
         turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset);
         SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset);
 
+        shuttlingTOFMultiplier = SmartDashboard.getNumber("Shuttling TOF Multiplier", shuttlingTOFMultiplier);
+        SmartDashboard.putNumber("Shuttling TOF Multiplier", shuttlingTOFMultiplier);
+
         if (phaseManager.isIdle()) {
             underLadder();
         } else {

diff --git a/src/main/java/frc/robot/constants/ShuttleInterpolation.java b/src/main/java/frc/robot/constants/ShuttleInterpolation.java
index 3d8074e1d31c8afec2d1361b326d941419cdf9ae..873db717f44aa2fd919684ad17ee3abbba9dec3c 100644 (file)
--- a/src/main/java/frc/robot/constants/ShuttleInterpolation.java
+++ b/src/main/java/frc/robot/constants/ShuttleInterpolation.java
@@ -19,13 +19,12 @@ public class ShuttleInterpolation {
         // we can be less aggressive: y = 0.65 * (1.34959x + 9.79618)
         // will likely be this that requires tuning.
         shooterVelocityMap.put(0.0, 9.0);
-        shooterVelocityMap.put(4.0, 12.8);
-        shooterVelocityMap.put(7.6, 19.0);
-        shooterVelocityMap.put(11.4, 28.1); // was 25.2 before
-        shooterVelocityMap.put(16.54, 44.8); // untested
+        shooterVelocityMap.put(4.0, 12.0 * 1.3); // tuned
+        shooterVelocityMap.put(8.0, 22.0 * 1.075); // tuned
+        shooterVelocityMap.put(16.0, 44.0); // untuned
         
         // always shoot at low angle to ground.
-        newHoodMap.put(0.0, 60.0);
-        newHoodMap.put(27.99, 60.0);
+        newHoodMap.put(0.0, 55.0);
+        newHoodMap.put(27.99, 55.0);
     }
 }