fixed some bugs in the shoot note visualizer in example

Shenzhen-Robotics-Alliance · Sep 10, 2024 · ab12186 · ab12186
1 parent 93138fe
commit ab12186
Show file tree

Hide file tree

Showing 6 changed files with 13 additions and 6 deletions.
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
@@ -379,7 +379,7 @@ public void configureButtonBindings() {
                 faceTargetWhileDrivingLowSpeed::chassisRotationInPosition,
                 ledStatusLight,
                 visualizerForShooter
-        );
+        ).ifNotePresent();
         driverXBox.rightTrigger(0.5).whileTrue(semiAutoAimAndShoot.deadlineWith(faceTargetWhileDrivingLowSpeed));
 
         driverXBox.rightBumper().whileTrue(new PathFindToPoseAndShootSequence(

diff --git a/...ple/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java b/...ple/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java
@@ -38,6 +38,7 @@ public AimAndShootSequence(
         );
     }
 
+    private final Intake intake;
     /**
      * creates a sequence to shoot at speaker in the following steps:
      * 1. runs a {@link PrepareToAim} command, until the shooter is prepared
@@ -55,6 +56,7 @@ public AimAndShootSequence(
             CompetitionFieldVisualizer visualizer) {
 
         super();
+        this.intake = intake;
         super.addRequirements(pitch, flyWheels, intake);
         super.addCommands(Commands.runOnce(intake::runIdle));
 
@@ -70,7 +72,11 @@ public AimAndShootSequence(
         super.addCommands(aimAtSpeakerContinuously.raceWith(waitForRightTimingAndShoot));
         super.addCommands(Commands.runOnce(() -> visualizer.addGamePieceOnFly(new Crescendo2024FieldObjects.NoteFlyingToShooter(
                 new Translation3d(drive.getPose().getX(), drive.getPose().getY(), 0.3),
-                0.5
+                8
         ))));
     }
+
+    public Command ifNotePresent() {
+        return this.onlyIf(intake::isNotePresent);
+    }
 }
diff --git a/...516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerContinuously.java b/...516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerContinuously.java
@@ -68,6 +68,7 @@ public void execute() {
 
     public boolean readyToShoot() {
         return shooterOptimizationRunning
+                && shooterOptimization.isTargetInRange(targetPositionSupplier.get(), drive.getPose().getTranslation())
                 && additionalCondition.getAsBoolean()
                 && flyWheels.flyWheelsReady()
                 && pitch.inPosition();

diff --git a/...ple/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerFactory.java b/...ple/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerFactory.java
@@ -23,7 +23,7 @@ public static Command shootAtSpeakerStill(HolonomicDriveSubsystem drive, Intake
                 chassisAimAtSpeaker::isFinished,
                 statusLight,
                 visualizer
-        );
+        ).ifNotePresent();
 
         final Command aimAtSpeakerStill = semiAutoAimAndShoot.deadlineWith(chassisAimAtSpeaker);
         aimAtSpeakerStill.addRequirements(drive, pitch, flyWheels);

diff --git a/...24-OffSeason/src/main/java/frc/robot/commands/shooter/PathFindToPoseAndShootSequence.java b/...24-OffSeason/src/main/java/frc/robot/commands/shooter/PathFindToPoseAndShootSequence.java
@@ -52,7 +52,7 @@ public PathFindToPoseAndShootSequence(
                         () -> isChassisSlowEnough(driveSubsystem),
                         statusLight,
                         visualizer
-                )
+                ).ifNotePresent()
         );
 
         super.addRequirements(driveSubsystem, pitch, flyWheels, intake);

diff --git a/...rc/main/java/frc/robot/utils/CompetitionFieldUtils/Objects/Crescendo2024FieldObjects.java b/...rc/main/java/frc/robot/utils/CompetitionFieldUtils/Objects/Crescendo2024FieldObjects.java
@@ -68,11 +68,11 @@ public String getTypeName() {
      * the flight is simulated by a simple linear animation
      * */
     public static class NoteFlyingToShooter extends GamePieceOnFlyDisplay {
-        public NoteFlyingToShooter(Translation3d shooterPosition, double flightTimeSeconds) {
+        public NoteFlyingToShooter(Translation3d shooterPosition, double speedMPS) {
             super(
                     shooterPosition,
                     FieldConstants.toCurrentAllianceTranslation(FieldConstants.SPEAKER_POSE_BLUE),
-                    flightTimeSeconds
+                    FieldConstants.toCurrentAllianceTranslation(FieldConstants.SPEAKER_POSE_BLUE).getDistance(shooterPosition) / speedMPS
             );
         }