added pass shot simulation and visualization

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(

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() {

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;
     }
 }

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())