From 1e49c760342fbb8f8c593027ac082d5b867b4fa5 Mon Sep 17 00:00:00 2001 From: zoe Date: Sun, 5 Apr 2026 22:26:31 -0700 Subject: [PATCH 1/7] add shuttle command unfortunately a little destructive w/ the generalization --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/subsystems/RobotCommandFactory.java | 108 +++++++++++++++--- .../java/frc/robot/utils/HubCalculations.java | 88 +++++++++++++- 4 files changed, 180 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6a11e5f1..89631cdc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -723,6 +723,7 @@ 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 CORNER_OFFSET = 1; // Corner offset for shuttle shots (how far inside the field boundary to aim, in meters) } public static class RobotConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc9cbe12..4195c469 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -225,7 +225,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) diff --git a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java index 180f3887..d1644a72 100644 --- a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java @@ -73,6 +73,12 @@ public class RobotCommandFactory { private final Supplier m_drivetrainAngleSupplier = () -> calculateShootOnTheFlyAngle(); + // Shuttle suppliers + private final Supplier m_shuttleShooterVelocitySupplier = () -> calculateShuttleShooterVelocity(); + private final Supplier m_shuttleShooterVelocityInitialSupplier = + () -> calculateShuttleShooterVelocity() + ShooterConstants.INTIAL_ADDITIONAL_VELOCITY; + private final Supplier m_shuttleAngleSupplier = () -> calculateShuttleAngle(); + private final Supplier m_dynamicShooterVelocitySupplier; private final Trigger m_readyToShootTrigger; @@ -343,14 +349,28 @@ public Command recycleFuelCommand() { ).withName("recycleFuelCommand"); } - public Command shuttleCommand() { - return shootWithoutVisionWithDisplacement( - () -> ShooterConstants.SHUTTLE_VELOCITY + ShooterConstants.INTIAL_ADDITIONAL_VELOCITY, - () -> ShooterConstants.SHUTTLE_VELOCITY, + public Command shuttleCommand(Supplier 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 + shootWithAngleCheck( + m_shuttleShooterVelocityInitialSupplier, + m_shuttleShooterVelocitySupplier, () -> ColumnConstants.SHUTTLE_VELOCITY, () -> IndexerConstants.SHUTTLE_VELOCITY, - () -> RollerConstants.ROLLER_VELOCITY - ).withName("shuttleCommand"); + () -> RollerConstants.ROLLER_VELOCITY, + m_shuttleAngleSupplier // Use shuttle angle supplier for angle checking + ) + ).withName("shuttleCommand"); + } + + public Command snapToShuttleTargetCommand(Supplier joystickValsSupplier) { + return m_drivetrainCommandFactory.snapToAngle( + joystickValsSupplier, + m_shuttleAngleSupplier + ).withName("snapToShuttleTargetCommand"); } public Command neutralModeCommand() { @@ -560,7 +580,7 @@ public Command shootWithVisionDynamic(Supplier initialShooterSupplier, S } /** - * Command that shoots with shooter, column, indexer velocity supplier + * Generalized command that shoots with shooter, column, indexer velocity supplier * Simultaneously runs the shooter, then runs column and indexer **once the drivetrain is at the correct angle** * ORIGINAL VERSION: Uses simple sequences - column/indexer/roller run continuously after conditions met * @@ -569,15 +589,16 @@ public Command shootWithVisionDynamic(Supplier initialShooterSupplier, S * @param columnSupplier Supplier for column velocity * @param indexerSupplier Supplier for indexer velocity * @param rollerSupplier Supplier for roller velocity + * @param angleSupplier Supplier for target angle to wait for * @return Command that shoots with given velocity suppliers */ - public Command shootWithVision(Supplier initialShooterSupplier, Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier, Supplier rollerSupplier) { + private Command shootWithAngleCheck(Supplier initialShooterSupplier, Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier, Supplier rollerSupplier, Supplier angleSupplier) { return Commands.parallel( // log isFuelShot Commands.run(() -> { SmartDashboard.putBoolean("robotCommandFactory/isColumnHappy", m_column.isMotorVelocityOverPercentToleranceTrigger(columnSupplier).getAsBoolean()); - SmartDashboard.putBoolean("robotCommandFactory/atAngleTrigger", m_drivetrainCommandFactory.atAngleTrigger(m_drivetrainAngleSupplier).getAsBoolean()); + SmartDashboard.putBoolean("robotCommandFactory/atAngleTrigger", m_drivetrainCommandFactory.atAngleTrigger(angleSupplier).getAsBoolean()); SmartDashboard.putBoolean("robotCommandFactory/isMotorVelocityWithinPercentTolerance", m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier).getAsBoolean()); }), @@ -592,7 +613,7 @@ public Command shootWithVision(Supplier initialShooterSupplier, Supplier Commands.sequence( m_column.offCommand() // wait until .until(m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier) // shooter at target velocity - .and(m_drivetrainCommandFactory.atAngleTrigger(m_drivetrainAngleSupplier))) // and facing hub + .and(m_drivetrainCommandFactory.atAngleTrigger(angleSupplier))) // and facing target .withTimeout(ShooterConstants.WAIT_FOR_SHOOTER_TIMEOUT), m_column.velocityCommand(columnSupplier)), @@ -600,7 +621,7 @@ public Command shootWithVision(Supplier initialShooterSupplier, Supplier Commands.sequence( m_indexer.offCommand() // wait until .until(m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier) // shooter at target velocity - .and(m_drivetrainCommandFactory.atAngleTrigger(m_drivetrainAngleSupplier))) // and facing hub + .and(m_drivetrainCommandFactory.atAngleTrigger(angleSupplier))) // and facing target .withTimeout(ShooterConstants.WAIT_FOR_SHOOTER_TIMEOUT), m_indexer.velocityCommand(indexerSupplier)), @@ -608,10 +629,26 @@ public Command shootWithVision(Supplier initialShooterSupplier, Supplier Commands.sequence( m_roller.offCommand() // wait until .until(m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier) // shooter at target velocity - .and(m_drivetrainCommandFactory.atAngleTrigger(m_drivetrainAngleSupplier))) // and facing hub + .and(m_drivetrainCommandFactory.atAngleTrigger(angleSupplier))) // and facing target .withTimeout(ShooterConstants.WAIT_FOR_SHOOTER_TIMEOUT), m_roller.velocityCommand(rollerSupplier)) - ).withName("shootWithVision"); + ).withName("shootWithAngleCheck"); + } + + /** + * Command that shoots with shooter, column, indexer velocity supplier + * Simultaneously runs the shooter, then runs column and indexer **once the drivetrain is at the correct angle** + * ORIGINAL VERSION: Uses simple sequences - column/indexer/roller run continuously after conditions met + * + * @param initialShooterSupplier Supplier for 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 with given velocity suppliers + */ + public Command shootWithVision(Supplier initialShooterSupplier, Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier, Supplier rollerSupplier) { + return shootWithAngleCheck(initialShooterSupplier, shooterSupplier, columnSupplier, indexerSupplier, rollerSupplier, m_drivetrainAngleSupplier).withName("shootWithVision"); } /** @@ -772,6 +809,51 @@ 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); + + // Look up target velocity from distance + Optional 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); } diff --git a/src/main/java/frc/robot/utils/HubCalculations.java b/src/main/java/frc/robot/utils/HubCalculations.java index f64486c3..34e30c22 100644 --- a/src/main/java/frc/robot/utils/HubCalculations.java +++ b/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,6 +45,18 @@ 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 targetLocationSupplier) { // 1. LATENCY COMPENSATION - Project robot position forward Translation2d futurePos = robotPose.getTranslation().plus( new Translation2d( @@ -52,8 +64,8 @@ public static Rotation2d calculateShootOnTheFlyAngle(Pose2d robotPose, ChassisSp 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.CORNER_OFFSET, + FieldConstants.fieldWidth - TeleopConstants.CORNER_OFFSET + ); + + // Right corner: low X, low Y + Translation2d rightCorner = new Translation2d( + TeleopConstants.CORNER_OFFSET, + TeleopConstants.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 shuttleTargetSupplier = () -> getClosestShuttleCorner(robotPose); + return calculateShootOnTheFlyAngleToTarget(robotPose, robotSpeeds, shuttleTargetSupplier); + } } From 8f7f2ff89752124e28a9f16b08d557b40af8cb2b Mon Sep 17 00:00:00 2001 From: zoe Date: Sun, 5 Apr 2026 22:33:18 -0700 Subject: [PATCH 2/7] intake while shuttling --- src/main/java/frc/robot/Constants.java | 6 +- .../robot/subsystems/RobotCommandFactory.java | 62 +++++++++++++------ 2 files changed, 48 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 89631cdc..5081b5ae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -267,6 +267,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; @@ -322,8 +324,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; @@ -375,7 +377,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 diff --git a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java index d1644a72..ca91dcd2 100644 --- a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java @@ -355,13 +355,12 @@ public Command shuttleCommand(Supplier joystickValsSupplier) { snapToShuttleTargetCommand(joystickValsSupplier), // Run shooter and feeding mechanisms with dynamic velocity based on distance // Only feed when at the correct angle - shootWithAngleCheck( + shootForShuttle( m_shuttleShooterVelocityInitialSupplier, m_shuttleShooterVelocitySupplier, () -> ColumnConstants.SHUTTLE_VELOCITY, () -> IndexerConstants.SHUTTLE_VELOCITY, - () -> RollerConstants.ROLLER_VELOCITY, - m_shuttleAngleSupplier // Use shuttle angle supplier for angle checking + () -> RollerConstants.SHUTTLE_VELOCITY ) ).withName("shuttleCommand"); } @@ -580,7 +579,7 @@ public Command shootWithVisionDynamic(Supplier initialShooterSupplier, S } /** - * Generalized command that shoots with shooter, column, indexer velocity supplier + * Command that shoots with shooter, column, indexer velocity supplier * Simultaneously runs the shooter, then runs column and indexer **once the drivetrain is at the correct angle** * ORIGINAL VERSION: Uses simple sequences - column/indexer/roller run continuously after conditions met * @@ -589,16 +588,15 @@ public Command shootWithVisionDynamic(Supplier initialShooterSupplier, S * @param columnSupplier Supplier for column velocity * @param indexerSupplier Supplier for indexer velocity * @param rollerSupplier Supplier for roller velocity - * @param angleSupplier Supplier for target angle to wait for * @return Command that shoots with given velocity suppliers */ - private Command shootWithAngleCheck(Supplier initialShooterSupplier, Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier, Supplier rollerSupplier, Supplier angleSupplier) { + public Command shootWithVision(Supplier initialShooterSupplier, Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier, Supplier rollerSupplier) { return Commands.parallel( // log isFuelShot Commands.run(() -> { SmartDashboard.putBoolean("robotCommandFactory/isColumnHappy", m_column.isMotorVelocityOverPercentToleranceTrigger(columnSupplier).getAsBoolean()); - SmartDashboard.putBoolean("robotCommandFactory/atAngleTrigger", m_drivetrainCommandFactory.atAngleTrigger(angleSupplier).getAsBoolean()); + SmartDashboard.putBoolean("robotCommandFactory/atAngleTrigger", m_drivetrainCommandFactory.atAngleTrigger(m_drivetrainAngleSupplier).getAsBoolean()); SmartDashboard.putBoolean("robotCommandFactory/isMotorVelocityWithinPercentTolerance", m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier).getAsBoolean()); }), @@ -613,7 +611,7 @@ private Command shootWithAngleCheck(Supplier initialShooterSupplier, Sup Commands.sequence( m_column.offCommand() // wait until .until(m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier) // shooter at target velocity - .and(m_drivetrainCommandFactory.atAngleTrigger(angleSupplier))) // and facing target + .and(m_drivetrainCommandFactory.atAngleTrigger(m_drivetrainAngleSupplier))) // and facing hub .withTimeout(ShooterConstants.WAIT_FOR_SHOOTER_TIMEOUT), m_column.velocityCommand(columnSupplier)), @@ -621,7 +619,7 @@ private Command shootWithAngleCheck(Supplier initialShooterSupplier, Sup Commands.sequence( m_indexer.offCommand() // wait until .until(m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier) // shooter at target velocity - .and(m_drivetrainCommandFactory.atAngleTrigger(angleSupplier))) // and facing target + .and(m_drivetrainCommandFactory.atAngleTrigger(m_drivetrainAngleSupplier))) // and facing hub .withTimeout(ShooterConstants.WAIT_FOR_SHOOTER_TIMEOUT), m_indexer.velocityCommand(indexerSupplier)), @@ -629,26 +627,54 @@ private Command shootWithAngleCheck(Supplier initialShooterSupplier, Sup Commands.sequence( m_roller.offCommand() // wait until .until(m_shooter.isMotorVelocityWithinPercentTolerance(shooterSupplier) // shooter at target velocity - .and(m_drivetrainCommandFactory.atAngleTrigger(angleSupplier))) // and facing target + .and(m_drivetrainCommandFactory.atAngleTrigger(m_drivetrainAngleSupplier))) // and facing hub .withTimeout(ShooterConstants.WAIT_FOR_SHOOTER_TIMEOUT), m_roller.velocityCommand(rollerSupplier)) - ).withName("shootWithAngleCheck"); + ).withName("shootWithVision"); } /** - * Command that shoots with shooter, column, indexer velocity supplier - * Simultaneously runs the shooter, then runs column and indexer **once the drivetrain is at the correct angle** - * ORIGINAL VERSION: Uses simple sequences - column/indexer/roller run continuously after conditions met + * Command for shuttle shooting - waits for correct angle before feeding + * Simultaneously runs the shooter, then runs column and indexer **once the drivetrain is at the shuttle angle** * - * @param initialShooterSupplier Supplier for shooter velocity + * @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 with given velocity suppliers + * @return Command that shoots for shuttle with given velocity suppliers */ - public Command shootWithVision(Supplier initialShooterSupplier, Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier, Supplier rollerSupplier) { - return shootWithAngleCheck(initialShooterSupplier, shooterSupplier, columnSupplier, indexerSupplier, rollerSupplier, m_drivetrainAngleSupplier).withName("shootWithVision"); + public Command shootForShuttle(Supplier initialShooterSupplier, Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier, Supplier 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"); } /** From a7e1085215682062bcb6b3e96f99a1543e57addb Mon Sep 17 00:00:00 2001 From: zoe Date: Sun, 5 Apr 2026 22:34:46 -0700 Subject: [PATCH 3/7] update comment --- src/main/java/frc/robot/subsystems/RobotCommandFactory.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java index ca91dcd2..ebbf4e30 100644 --- a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java @@ -635,7 +635,8 @@ public Command shootWithVision(Supplier initialShooterSupplier, Supplier /** * Command for shuttle shooting - waits for correct angle before feeding - * Simultaneously runs the shooter, then runs column and indexer **once the drivetrain is at the shuttle angle** + * 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 From 05d84e94e6083bae85eb00f82fcda4605fc3d583 Mon Sep 17 00:00:00 2001 From: zoe Date: Mon, 6 Apr 2026 17:36:19 -0700 Subject: [PATCH 4/7] log shuttle distabce --- src/main/java/frc/robot/subsystems/RobotCommandFactory.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java index f206a312..dbd8871c 100644 --- a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java @@ -865,6 +865,8 @@ private Double calculateShuttleShooterVelocity() { Pose2d robotPose = m_drivetrain.getState().Pose; double distanceToCorner = HubCalculations.distanceToShuttleCorner(robotPose); + SmartDashboard.putNumber("robotCommandFactory/shuttleDistance", distanceToCorner); + // Look up target velocity from distance Optional velocityOptional = ShooterLookupTable.getVelocityForDistance(distanceToCorner); From d54c558a4be1a765da0877f642e9bea03e7c8699 Mon Sep 17 00:00:00 2001 From: zoe Date: Mon, 6 Apr 2026 17:36:27 -0700 Subject: [PATCH 5/7] split corner offset --- src/main/java/frc/robot/Constants.java | 4 +++- src/main/java/frc/robot/utils/HubCalculations.java | 8 ++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c47c11a5..f85220ab 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -731,7 +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 CORNER_OFFSET = 1; // Corner offset for shuttle shots (how far inside the field boundary to aim, in meters) + 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 { diff --git a/src/main/java/frc/robot/utils/HubCalculations.java b/src/main/java/frc/robot/utils/HubCalculations.java index 34e30c22..b2873916 100644 --- a/src/main/java/frc/robot/utils/HubCalculations.java +++ b/src/main/java/frc/robot/utils/HubCalculations.java @@ -100,14 +100,14 @@ private static Translation2d[] getShuttleCorners() { // Alliance corners are on our side (low X value) // Left corner: low X, high Y Translation2d leftCorner = new Translation2d( - TeleopConstants.CORNER_OFFSET, - FieldConstants.fieldWidth - TeleopConstants.CORNER_OFFSET + TeleopConstants.X_CORNER_OFFSET, + FieldConstants.fieldWidth - TeleopConstants.Y_CORNER_OFFSET ); // Right corner: low X, low Y Translation2d rightCorner = new Translation2d( - TeleopConstants.CORNER_OFFSET, - TeleopConstants.CORNER_OFFSET + TeleopConstants.X_CORNER_OFFSET, + TeleopConstants.Y_CORNER_OFFSET ); return new Translation2d[] { leftCorner, rightCorner }; From 99c4791207bc2fdd4eeddeec28cc32db8337af39 Mon Sep 17 00:00:00 2001 From: zoe Date: Mon, 6 Apr 2026 17:36:45 -0700 Subject: [PATCH 6/7] add bogus LUT entry for shuttle --- src/main/java/frc/robot/utils/ShooterLookupTable.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/utils/ShooterLookupTable.java b/src/main/java/frc/robot/utils/ShooterLookupTable.java index ec4af159..881c5811 100644 --- a/src/main/java/frc/robot/utils/ShooterLookupTable.java +++ b/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 } /** From caea22d53976c192993d39dd0be8283de5778104 Mon Sep 17 00:00:00 2001 From: zoe Date: Mon, 6 Apr 2026 17:36:55 -0700 Subject: [PATCH 7/7] lower shooter column drive current limits --- src/main/java/frc/robot/Constants.java | 10 +++++----- src/main/java/frc/robot/generated/TunerConstants.java | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f85220ab..8d179bc1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -360,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; @@ -445,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; diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 203b13d7..685d675c 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/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()