diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f55e41b0..8d179bc1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 40088986..d1716d1a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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) 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() diff --git a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java index cef7e518..dbd8871c 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 Supplier 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 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 joystickValsSupplier) { + return m_drivetrainCommandFactory.snapToAngle( + joystickValsSupplier, + m_shuttleAngleSupplier + ).withName("snapToShuttleTargetCommand"); } public Command neutralModeCommand() { @@ -626,6 +645,51 @@ public Command shootWithVision(Supplier 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 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"); + } + /** * 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 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..b2873916 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.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 shuttleTargetSupplier = () -> getClosestShuttleCorner(robotPose); + return calculateShootOnTheFlyAngleToTarget(robotPose, robotSpeeds, shuttleTargetSupplier); + } } 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 } /**