diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java index d51b43d..b9cf1c6 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java @@ -393,7 +393,7 @@ public void configureButtonBindings() { .whileTrue(Commands.run(() -> joystickDrive.setCurrentRotationalMaintenance( FieldConstants.toCurrentAllianceRotation(Rotation2d.fromDegrees(180)) ))) - .onFalse(FeedShot.shootFeed(pitch, flyWheels, intake)); + .onFalse(FeedShot.shootFeed(pitch, flyWheels, intake, fieldSimulation, drive)); /* aim and shoot command */ final JoystickDriveAndAimAtTarget faceTargetWhileDrivingLowSpeed = new JoystickDriveAndAimAtTarget( diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java index db38139..7c6d462 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java @@ -67,13 +67,19 @@ public AimAndShootSequence( final AimAtSpeakerContinuously aimAtSpeakerContinuously = new AimAtSpeakerContinuously( flyWheels, pitch, ledStatusLight, shooterOptimization, drive, targetPositionSupplier, externalShootCondition ); - final Command waitForRightTimingAndShoot = Commands.waitUntil(aimAtSpeakerContinuously::readyToShoot) - .andThen(intake.executeLaunch()); - super.addCommands(aimAtSpeakerContinuously.raceWith(waitForRightTimingAndShoot)); - super.addCommands(Commands.runOnce(() -> visualizer.addGamePieceOnFly(new Crescendo2024FieldObjects.NoteFlyingToShooter( + + /* visualization */ + final Command visualizeNote; + if (visualizer == null) + visualizeNote = Commands.none(); + else visualizeNote = Commands.runOnce(() -> visualizer.addGamePieceOnFly(new Crescendo2024FieldObjects.NoteFlyingToSpeaker( new Translation3d(drive.getPose().getX(), drive.getPose().getY(), 0.3), shooterOptimization.getFlightTimeSeconds(targetPositionSupplier.get(), robotScoringPositionSupplier.get()) - )))); + ))); + + final Command waitForRightTimingAndShoot = Commands.waitUntil(aimAtSpeakerContinuously::readyToShoot) + .andThen(intake.executeLaunch().alongWith(visualizeNote)); + super.addCommands(aimAtSpeakerContinuously.raceWith(waitForRightTimingAndShoot)); } public Command ifNotePresent() { diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FeedShot.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FeedShot.java index 2ff7da2..d97632a 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FeedShot.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FeedShot.java @@ -1,11 +1,16 @@ package frc.robot.commands.shooter; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.constants.PitchConstants; +import frc.robot.subsystems.drive.HolonomicDriveSubsystem; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.FlyWheels; import frc.robot.subsystems.shooter.Pitch; +import frc.robot.utils.CompetitionFieldUtils.Objects.Crescendo2024FieldObjects; +import frc.robot.utils.CompetitionFieldUtils.Simulations.CompetitionFieldSimulation; public class FeedShot { public static Command prepareToFeedForever(Pitch pitch, FlyWheels flyWheels) { @@ -21,11 +26,36 @@ public static Command prepareToFeedUntilReady(Pitch pitch, FlyWheels flyWheels) ); } - public static Command shootFeed(Pitch pitch, FlyWheels flyWheels, Intake intake) { - return prepareToFeedUntilReady(pitch, flyWheels).withTimeout(1). - andThen( - intake.executeLaunch().withTimeout(1) - .deadlineWith(prepareToFeedForever(pitch, flyWheels)) - ); + public static Command shootFeed(Pitch pitch, FlyWheels flyWheels, Intake intake, CompetitionFieldSimulation simulation, HolonomicDriveSubsystem driveSubsystem) { + final SequentialCommandGroup shootFeed = new SequentialCommandGroup(); + final boolean[] noteInShooter = new boolean[] {false}; + + shootFeed.addRequirements(pitch, flyWheels, intake); + + shootFeed.addCommands(Commands.runOnce(() -> noteInShooter[0] = intake.isNotePresent())); + + shootFeed.addCommands(prepareToFeedUntilReady(pitch, flyWheels) + .withTimeout(0.5) + ); + + shootFeed.addCommands(intake.executeLaunch() + .withTimeout(2) + .deadlineWith(prepareToFeedForever(pitch, flyWheels)) + ); + + /* visualization */ + final Command simulationCommand; + if (simulation == null) simulationCommand = Commands.none(); + else simulationCommand = Commands.runOnce(() -> simulation.addGamePiece(new Crescendo2024FieldObjects.FeedShotLowNote( + driveSubsystem.getPose() + .getTranslation() + .plus(new Translation2d(0.5, 0) + .rotateBy(driveSubsystem.getFacing()) + ), + driveSubsystem.getFacing() + ))); + shootFeed.addCommands(simulationCommand.onlyIf(() -> noteInShooter[0])); + + return shootFeed; } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/ScoreAmp.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/ScoreAmp.java index fb7fb61..d495563 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/ScoreAmp.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/ScoreAmp.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SelectCommand; import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; import frc.robot.subsystems.intake.Intake; @@ -87,10 +86,9 @@ public Command withVisualization(CompetitionFieldVisualizer visualizer, Competit private static final double SHOOTER_HEIGHT_AMP = 0.4, - AMP_HEIGHT = 0.8, - AMP_INITIAL_VELOCITY = 4, + AMP_INITIAL_VELOCITY = 3.5, AMP_FLIGHT_TIME_SECONDS = 0.8; - private static final Translation3d AMP_POSITION = new Translation3d(1.81, 8.41, AMP_HEIGHT); + private static final Translation3d AMP_POSITION = new Translation3d(1.81, 8.35, 0); private static final Pose2d CORRECT_SCORE_AMP_ROBOT_POSE_BLUE = new Pose2d(1.81, 7.71, Rotation2d.fromDegrees(-90)); private static final class AmpSuccessNote extends GamePieceOnFlyDisplay { public AmpSuccessNote(Translation2d robotPosition) { @@ -112,8 +110,9 @@ public Pose3d getPose3d() { * the amplified note follows the free-fall law * h = h0 + v*t - 1/2*t^2*g; * */ - final double height = SHOOTER_HEIGHT_AMP + AMP_INITIAL_VELOCITY * getT() - - 1.0/2.0 * getT() * getT() * 10; + final double t = getTimeSinceLaunchSeconds(), + height = SHOOTER_HEIGHT_AMP + AMP_INITIAL_VELOCITY * t + - 1.0/2.0 * t * t * 10; return new Pose3d( new Translation3d(super.getPose3d().getX(), super.getPose3d().getY(), height), new Rotation3d(Math.toRadians(90), 0, 0) @@ -157,8 +156,9 @@ public String getTypeName() { @Override public Pose3d getPose3d() { - final double height = SHOOTER_HEIGHT_AMP + AMP_INITIAL_VELOCITY * getT() - - 1.0/2.0 * getT() * getT() * 10; + final double t = getTimeSinceLaunchSeconds(), + height = SHOOTER_HEIGHT_AMP + AMP_INITIAL_VELOCITY * t + - 1.0/2.0 * t * t * 10; return new Pose3d( new Translation3d(super.getPose3d().getX(), super.getPose3d().getY(), height), new Rotation3d(0, Math.toRadians(90), robotFacingWhenLaunching.getRadians())