improve shuttle mode

src/main/java/frc/robot/Constants.java

@@ -273,6 +273,8 @@ public static class RollerConstants {
     public static double RECYCLE_VELOCITY = 70.0;
     public static double INTAKE_VELOCITY = 80.0;
     public static double SHOOT_VELOCITY = 70.0;
+    public static double SHUTTLE_VELOCITY = INTAKE_VELOCITY;
+
 
     // Testing velocities
     public static double TESTING_VELOCITY = 0.0;
@@ -328,8 +330,8 @@ public static class IndexerConstants {
     public static double MANUAL_BACK_VELOCITY = -80.0;
     public static double OUTTAKE_VELOCITY = -80.0;
     public static double RECYCLE_VELOCITY = 50.0;
-    public static double SHUTTLE_VELOCITY = 50.0;
     public static double INTAKE_VELOCITY = 50.0;
+    public static double SHUTTLE_VELOCITY = INTAKE_VELOCITY;
 
     // Testing velocities
     public static double TESTING_VELOCITY = 0.0;
@@ -358,8 +360,8 @@ public static class ColumnConstants {
     // Motor limits
     public static final int INFLUENCER_STATOR_LIMIT = 80;
     public static final int FOLLOWER_STATOR_LIMIT = 80;
-    public static final int INFLUENCER_SUPPLY_LIMIT = 40;
-    public static final int FOLLOWER_SUPPLY_LIMIT = 40;
+    public static final int INFLUENCER_SUPPLY_LIMIT = 35;
+    public static final int FOLLOWER_SUPPLY_LIMIT = INFLUENCER_SUPPLY_LIMIT;
 
     public static final boolean IS_INFLUENCER_INVERTED = false;
 
@@ -381,7 +383,7 @@ public static class ColumnConstants {
     public static double MANUAL_BACK_VELOCITY = -70.0;
     public static double OUTTAKE_VELOCITY = -70.0;
     public static double RECYCLE_VELOCITY = 70.0;
-    public static double SHUTTLE_VELOCITY = 70.0;
+    public static double SHUTTLE_VELOCITY = SHOOT_VELOCITY;
     public static double INTAKE_VELOCITY = -40.0;
 
     // Testing velocities
@@ -443,9 +445,9 @@ public static class ShooterConstants {
     public static final int INFLUENCER_MOTOR_STATOR_LIMIT = 80;
     public static final int FOLLOWER_MOTOR_STATOR_LIMIT = 80;
     public static final int THIRD_MOTOR_STATOR_LIMIT = 80;
-    public static final int INFLUENCER_MOTOR_SUPPLY_LIMIT = 40;
-    public static final int FOLLOWER_MOTOR_SUPPLY_LIMIT = 40;
-    public static final int THIRD_MOTOR_SUPPLY_LIMIT = 40;
+    public static final int INFLUENCER_MOTOR_SUPPLY_LIMIT = 35;
+    public static final int FOLLOWER_MOTOR_SUPPLY_LIMIT = INFLUENCER_MOTOR_SUPPLY_LIMIT;
+    public static final int THIRD_MOTOR_SUPPLY_LIMIT = INFLUENCER_MOTOR_SUPPLY_LIMIT;
 
     public static final boolean IS_INFLUENCER_INVERTED = false;
 
@@ -729,6 +731,9 @@ public static class AutoConstants {
 
   public static class TeleopConstants {
     public static final double LATENCY_COMPENSATION = 0.25; // compensate for shooter spin up + code processing time, etc. TODO: tune
+    public static final double X_CORNER_OFFSET = 0.5; // Corner offset for shuttle shots (how far inside the field boundary to aim, in meters)
+    public static final double Y_CORNER_OFFSET = 2; // Corner offset for shuttle shots (how far inside the field boundary to aim, in meters)
+
   }
 
   public static class RobotConstants {

src/main/java/frc/robot/RobotContainer.java

@@ -229,7 +229,7 @@ private void configureBindingsCompetition() {
     // right bumper: slowmode
     m_drivetrainCommandFactory.setSlowmodeButton(m_driverJoystick.rightBumper());
     // left trigger: shuttle
-    m_driverJoystick.leftTrigger().whileTrue(m_robotCommandFactory.shuttleCommand());
+    m_driverJoystick.leftTrigger().whileTrue(m_robotCommandFactory.shuttleCommand(m_driverTranslationJoystickValsSupplier));
     m_driverJoystick.leftTrigger().onTrue(
       Commands.runOnce(() -> scoreMode = false));
     // right trigger: outtake / everything backwards (voltage -5)

src/main/java/frc/robot/generated/TunerConstants.java

@@ -85,7 +85,7 @@ public class TunerConstants {
             new CurrentLimitsConfigs()
                 .withStatorCurrentLimit(Amps.of(80))
                 .withStatorCurrentLimitEnable(true)
-                .withSupplyCurrentLimit(50)
+                .withSupplyCurrentLimit(45)
                 .withSupplyCurrentLimitEnable(true)
         );
     private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()

src/main/java/frc/robot/subsystems/RobotCommandFactory.java

@@ -73,6 +73,12 @@ public class RobotCommandFactory {
 
   private final Supplier<Rotation2d> m_drivetrainAngleSupplier = () -> calculateShootOnTheFlyAngle();
 
+  // Shuttle suppliers
+  private final Supplier<Double> m_shuttleShooterVelocitySupplier = () -> calculateShuttleShooterVelocity();
+  private final Supplier<Double> m_shuttleShooterVelocityInitialSupplier =
+    () -> calculateShuttleShooterVelocity() + ShooterConstants.INTIAL_ADDITIONAL_VELOCITY;
+  private final Supplier<Rotation2d> m_shuttleAngleSupplier = () -> calculateShuttleAngle();
+
   private final Supplier<Double> m_dynamicShooterVelocitySupplier;
   private final Supplier<Boolean> m_scoreModeSupplier;
   private final Trigger m_readyToShootTrigger;
@@ -349,14 +355,27 @@ public Command recycleFuelCommand() {
     ).withName("recycleFuelCommand");
   }
 
-  public Command shuttleCommand() {
-    return shootWithoutVisionWithDisplacement(
-        () -> ShooterConstants.SHUTTLE_VELOCITY + ShooterConstants.INTIAL_ADDITIONAL_VELOCITY,
-        () -> ShooterConstants.SHUTTLE_VELOCITY,
+  public Command shuttleCommand(Supplier<JoystickVals> joystickValsSupplier) {
+    return Commands.parallel(
+      // Snap to shuttle target corner while allowing driver translation
+      snapToShuttleTargetCommand(joystickValsSupplier),
+      // Run shooter and feeding mechanisms with dynamic velocity based on distance
+      // Only feed when at the correct angle
+      shootForShuttle(
+        m_shuttleShooterVelocityInitialSupplier,
+        m_shuttleShooterVelocitySupplier,
         () -> ColumnConstants.SHUTTLE_VELOCITY,
         () -> IndexerConstants.SHUTTLE_VELOCITY,
-        () -> RollerConstants.ROLLER_VELOCITY
-      ).withName("shuttleCommand");
+        () -> RollerConstants.SHUTTLE_VELOCITY
+      )
+    ).withName("shuttleCommand");
+  }
+
+  public Command snapToShuttleTargetCommand(Supplier<JoystickVals> joystickValsSupplier) {
+    return m_drivetrainCommandFactory.snapToAngle(
+      joystickValsSupplier,
+      m_shuttleAngleSupplier
+    ).withName("snapToShuttleTargetCommand");
   }
 
   public Command neutralModeCommand() {
@@ -626,6 +645,51 @@ public Command shootWithVision(Supplier<Double> initialShooterSupplier, Supplier
     ).withName("shootWithVision");
   }
 
+  /**
+   * Command for shuttle shooting - waits for correct angle before feeding
+   * Simultaneously runs the shooter, then runs column **once the drivetrain is at the shuttle angle**
+   * Note that the indexer and roller run at all times so intaking at the same time is possible
+   *
+   * @param initialShooterSupplier Supplier for initial shooter velocity
+   * @param shooterSupplier Supplier for shooter velocity
+   * @param columnSupplier Supplier for column velocity
+   * @param indexerSupplier Supplier for indexer velocity
+   * @param rollerSupplier Supplier for roller velocity
+   * @return Command that shoots for shuttle with given velocity suppliers
+   */
+  public Command shootForShuttle(Supplier<Double> initialShooterSupplier, Supplier<Double> shooterSupplier, Supplier<Double> columnSupplier, Supplier<Double> indexerSupplier, Supplier<Double> rollerSupplier) {
+    return Commands.parallel(
+
+      // log isFuelShot
+      Commands.run(() -> {
+        SmartDashboard.putBoolean("robotCommandFactory/isColumnHappy", m_column.isMotorVelocityOverPercentToleranceTrigger(columnSupplier).getAsBoolean());
+        SmartDashboard.putBoolean("robotCommandFactory/atShuttleAngleTrigger", m_drivetrainCommandFactory.atAngleTrigger(m_shuttleAngleSupplier).getAsBoolean());
+        SmartDashboard.putBoolean("robotCommandFactory/isMotorVelocityWithinPercentTolerance", m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier).getAsBoolean());
+      }),
+
+      // shooter
+      Commands.sequence(
+        m_shooter.shooterVelocityCommand(initialShooterSupplier)
+          .until(m_column.isMotorVelocityOverPercentToleranceTrigger(columnSupplier))
+          .withTimeout(ShooterConstants.FUEL_SHOT_TIMEOUT),
+        m_shooter.shooterVelocityCommand(shooterSupplier)),  // run shooter at given velocity
+
+      // column
+      Commands.sequence(
+        m_column.offCommand() // wait until
+          .until(m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier) // shooter at target velocity
+            .and(m_drivetrainCommandFactory.atAngleTrigger(m_shuttleAngleSupplier))) // and facing shuttle corner
+          .withTimeout(ShooterConstants.WAIT_FOR_SHOOTER_TIMEOUT),
+        m_column.velocityCommand(columnSupplier)),
+
+      // indexer -- always run 
+      m_indexer.velocityCommand(indexerSupplier),
+
+      // roller -- always run 
+      m_roller.velocityCommand(rollerSupplier)
+    ).withName("shootForShuttle");
+  }
+
   /**
    * Command that shoots based on set[mechanism]Velocity()
    * Simultaneously runs the shooter, then runs column, indexer and roller 
@@ -793,6 +857,53 @@ private Rotation2d calculateShootOnTheFlyAngle() {
     return drivetrainAngle;
   }
 
+  /**
+   * Calculates the shooter velocity based on distance to shuttle corner.
+   */
+  private Double calculateShuttleShooterVelocity() {
+    // Calculate distance to closest shuttle corner
+    Pose2d robotPose = m_drivetrain.getState().Pose;
+    double distanceToCorner = HubCalculations.distanceToShuttleCorner(robotPose);
+
+    SmartDashboard.putNumber("robotCommandFactory/shuttleDistance", distanceToCorner); 
+
+    // Look up target velocity from distance
+    Optional<Double> velocityOptional = ShooterLookupTable.getVelocityForDistance(distanceToCorner);
+
+    if (velocityOptional.isPresent()) {
+      return velocityOptional.get();
+    } else {
+      return 0.0; // Distance out of range
+    }
+  }
+
+  /**
+   * Calculates the required field-relative drivetrain angle for shuttling.
+   * Uses SOTM math to compensate for robot velocity.
+   */
+  private Rotation2d calculateShuttleAngle() {
+    // Get robot state
+    Pose2d robotPose = m_drivetrain.getState().Pose;
+    ChassisSpeeds robotSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(
+      m_drivetrain.getState().Speeds,
+      robotPose.getRotation());
+
+    // Calculate shuttle angle using HubCalculations
+    Rotation2d shuttleAngle = HubCalculations.calculateShuttleAngle(robotPose, robotSpeeds);
+
+    if (shuttleAngle == null) {
+      // Distance out of range, fall back to simple angle to closest corner
+      Translation2d closestCorner = HubCalculations.getClosestShuttleCorner(robotPose);
+      Translation2d toCorner = closestCorner.minus(robotPose.getTranslation());
+      return toCorner.getAngle();
+    }
+
+    SmartDashboard.putNumber("robotCommandFactory/shuttleAngleDegrees", shuttleAngle.getDegrees());
+    SmartDashboard.putNumber("robotCommandFactory/shuttleAngleRadians", shuttleAngle.getRadians());
+
+    return shuttleAngle;
+  }
+
   public double getDistanceToHub() {
     return HubCalculations.distanceToHub(m_drivetrain.getState().Pose);
   }

src/main/java/frc/robot/utils/HubCalculations.java

@@ -25,7 +25,7 @@ public static Rotation2d angleToHub(Pose2d robotPose) {
         if (translationToTarget.getNorm() < DrivetrainConstants.SNAP_TO_TARGET_DISTANCE_THRESHOLD) {
             return robotPose.getRotation();
         }
-        
+
         return hub.minus(robotPose.getTranslation()).getAngle();
     }
 
@@ -45,15 +45,27 @@ public static double distanceToHub(Pose2d robotPose) {
      * @return Rotation2d drivetrain angle, or null if distance is out of range
      */
     public static Rotation2d calculateShootOnTheFlyAngle(Pose2d robotPose, ChassisSpeeds robotSpeeds) {
+        return calculateShootOnTheFlyAngleToTarget(robotPose, robotSpeeds, hubTranslationSupplier);
+    }
+
+    /**
+     * Generalized shoot-on-the-fly angle calculation for any target location.
+     *
+     * @param robotPose Current robot pose
+     * @param robotSpeeds Current robot speeds (field-relative)
+     * @param targetLocationSupplier Supplier for target location on the field
+     * @return Rotation2d drivetrain angle, or null if distance is out of range
+     */
+    private static Rotation2d calculateShootOnTheFlyAngleToTarget(Pose2d robotPose, ChassisSpeeds robotSpeeds, Supplier<Translation2d> targetLocationSupplier) {
         // 1. LATENCY COMPENSATION - Project robot position forward
         Translation2d futurePos = robotPose.getTranslation().plus(
             new Translation2d(
                 robotSpeeds.vxMetersPerSecond * TeleopConstants.LATENCY_COMPENSATION,
                 robotSpeeds.vyMetersPerSecond * TeleopConstants.LATENCY_COMPENSATION));
 
         // 2. GET TARGET VECTOR
-        Translation2d goalLocation = hubTranslationSupplier.get();
-        Translation2d targetVec = goalLocation.minus(futurePos);
+        Translation2d targetLocation = targetLocationSupplier.get();
+        Translation2d targetVec = targetLocation.minus(futurePos);
         double distance = targetVec.getNorm();
 
         // 3. GET EXIT VELOCITY FROM LOOKUP TABLE
@@ -62,8 +74,7 @@ public static Rotation2d calculateShootOnTheFlyAngle(Pose2d robotPose, ChassisSp
             return null; // Distance out of range
         }
 
-        // Convert RPM to m/s exit velocity
-        // Assuming the RPM is the flywheel speed and you need to convert to ball exit velocity
+        // Convert RPS to m/s exit velocity
         double exitVelocityMPS = rpmOptional.get() * ShooterConstants.SHOOTER_RPS_TO_MPS * ShooterConstants.SHOOTER_SLIP_FACTOR;
 
         // 4. CALCULATE HORIZONTAL VELOCITY from exit velocity and fixed shooter angle
@@ -78,4 +89,71 @@ public static Rotation2d calculateShootOnTheFlyAngle(Pose2d robotPose, ChassisSp
 
         return targetVec.div(distance).times(idealHorizontalSpeed).minus(robotVelVec).getAngle();
     }
+
+    /**
+     * Gets the shuttle target corners with offset (blue alliance perspective).
+     * Returns two corners on the alliance side (low X value).
+     *
+     * @return Array of two Translation2d representing [left corner, right corner]
+     */
+    private static Translation2d[] getShuttleCorners() {
+        // Alliance corners are on our side (low X value)
+        // Left corner: low X, high Y
+        Translation2d leftCorner = new Translation2d(
+            TeleopConstants.X_CORNER_OFFSET,
+            FieldConstants.fieldWidth - TeleopConstants.Y_CORNER_OFFSET
+        );
+
+        // Right corner: low X, low Y
+        Translation2d rightCorner = new Translation2d(
+            TeleopConstants.X_CORNER_OFFSET,
+            TeleopConstants.Y_CORNER_OFFSET
+        );
+
+        return new Translation2d[] { leftCorner, rightCorner };
+    }
+
+    /**
+     * Finds the closest shuttle corner to the robot.
+     *
+     * @param robotPose Current robot pose
+     * @return Translation2d of the closest corner (alliance-flipped)
+     */
+    public static Translation2d getClosestShuttleCorner(Pose2d robotPose) {
+        Translation2d[] corners = getShuttleCorners();
+
+        // Apply alliance flip to corners
+        Translation2d leftCorner = AllianceFlipUtil.apply(corners[0]);
+        Translation2d rightCorner = AllianceFlipUtil.apply(corners[1]);
+
+        // Find closest corner
+        double distToLeft = robotPose.getTranslation().getDistance(leftCorner);
+        double distToRight = robotPose.getTranslation().getDistance(rightCorner);
+
+        return distToLeft < distToRight ? leftCorner : rightCorner;
+    }
+
+    /**
+     * Calculates the distance to the closest shuttle corner.
+     *
+     * @param robotPose Current robot pose
+     * @return Distance in meters to the closest shuttle corner
+     */
+    public static double distanceToShuttleCorner(Pose2d robotPose) {
+        Translation2d closestCorner = getClosestShuttleCorner(robotPose);
+        return robotPose.getTranslation().getDistance(closestCorner);
+    }
+
+    /**
+     * Calculates the required drivetrain angle for shuttling with SOTM compensation.
+     * Aims at the closest corner of the field.
+     *
+     * @param robotPose Current robot pose
+     * @param robotSpeeds Current robot speeds (field-relative)
+     * @return Rotation2d drivetrain angle, or null if distance is out of range
+     */
+    public static Rotation2d calculateShuttleAngle(Pose2d robotPose, ChassisSpeeds robotSpeeds) {
+        Supplier<Translation2d> shuttleTargetSupplier = () -> getClosestShuttleCorner(robotPose);
+        return calculateShootOnTheFlyAngleToTarget(robotPose, robotSpeeds, shuttleTargetSupplier);
+    }
 }

src/main/java/frc/robot/utils/ShooterLookupTable.java

@@ -31,6 +31,7 @@ public class ShooterLookupTable {
     DISTANCE_TO_VELOCITY_MAP.put(4.5, 57.);   
     DISTANCE_TO_VELOCITY_MAP.put(5.1, 63.);   // corner
     DISTANCE_TO_VELOCITY_MAP.put(5.6, 65.);   // bogus
+    DISTANCE_TO_VELOCITY_MAP.put(10.0, 65.);   // bogus for shuttling
   }
 
   /**