enhance auto shooter speeds by making them run a raw triplet calculat…

…ion rather than setting speeds to a flat 2500
Patribots4738 · Apr 13, 2024 · 6f34db4 · 6f34db4
1 parent 67fce33
commit 6f34db4
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -224,7 +224,10 @@ public RobotContainer() {
         shooter.setDefaultCommand(
             pieceControl.getAutomaticShooterSpeeds(
                 swerve::getPose,
-                () -> driver.getLeftTrigger() || (!OIConstants.SINGLE_DRIVER_MODE && operator.getLeftBumper())    
+                () -> driver.getLeftTrigger() 
+                || driver.getLeftBumper() 
+                || (!OIConstants.SINGLE_DRIVER_MODE 
+                    && operator.getLeftBumper())
             )
         );
 

diff --git a/src/main/java/frc/robot/commands/managers/PieceControl.java b/src/main/java/frc/robot/commands/managers/PieceControl.java
@@ -21,7 +21,6 @@
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Intake;
-import frc.robot.subsystems.PicoColorSensor;
 import frc.robot.subsystems.Ampper;
 import frc.robot.util.Constants.FieldConstants;
 import frc.robot.util.Constants.ShooterConstants;
@@ -476,20 +475,32 @@ public Command setShooterModeCommand(boolean newShooterMode) {
     // Within a range of the [red circle](https://www.desmos.com/calculator/cu3ocssv5d)
     public Command getAutomaticShooterSpeeds(Supplier<Pose2d> robotPose, BooleanSupplier intaking) {
         return new ActiveConditionalCommand(
-            Commands.runOnce(
-                () -> shooterCmds.setSpeeds(ShooterConstants.DEFAULT_RPM), 
+            Commands.run(
+                () -> 
+                    shooterCmds.setSpeeds(
+                        (Robot.currentTimestamp - RobotContainer.gameModeStart < 7
+                            && Robot.gameMode == GameMode.TELEOP 
+                            && DriverStation.isFMSAttached())
+                        ? shooterCmds.shooterCalc.calculatePassTriplet(
+                            robotPose.get()
+                        ).getSpeeds()
+                        : shooterCmds.shooterCalc.calculateSpeakerTriplet(
+                            robotPose.get().getTranslation()
+                        ).getSpeeds()
+                    ),
                 shooterCmds.getShooter()
             ),
             shooterCmds.stopShooter(),
             () -> 
-                (((piPico.hasNoteShooter() 
+                (((piPico.hasNoteShooter() || piPico.hasNoteElevator()
                         && RobotContainer.distanceToSpeakerMeters < FieldConstants.AUTOMATIC_SHOOTER_DISTANCE_RADIUS)
-                    || (RobotContainer.distanceToSpeakerMeters < 3.4 && intaking.getAsBoolean() && elevator.getDesiredPosition() <= 0.1))
+                    || (RobotContainer.distanceToSpeakerMeters < 3.4 
+                        && intaking.getAsBoolean() 
+                        && elevator.getDesiredPosition() < ElevatorConstants.NOTE_FIX_POS))
 
-                || (Robot.currentTimestamp - RobotContainer.gameModeStart < 7 
+                || (Robot.currentTimestamp - RobotContainer.gameModeStart < 7
                     && Robot.gameMode == GameMode.TELEOP 
-                    && DriverStation.isFMSAttached()))
-                && RobotController.getBatteryVoltage() > 10)
+                    && DriverStation.isFMSAttached())))
             .onlyIf(() -> Robot.gameMode != GameMode.TEST);
     }