put lowpass back on right stick with positional condition, remove rob…

…ot velocity from calculating robot angle to speaker to slightly decrease alignment time
Patribots4738 · Oct 5, 2024 · ee600da · ee600da
1 parent a44988c
commit ee600da
Show file tree

Hide file tree

Showing 7 changed files with 25 additions and 28 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -451,7 +451,7 @@ private void configureCommonDriverBindings(PatriBoxController controller) {
                         // This runs SWD on heading control 
                         // and shooting-while-still on shooter
                         alignmentCmds.wingRotationalAlignment(controller::getLeftX, controller::getLeftY, robotRelativeSupplier),
-                        alignmentCmds.preparePassCommand(controller::getLeftX, controller::getLeftY, robotRelativeSupplier, false),
+                        alignmentCmds.preparePassCommand(controller::getLeftX, controller::getLeftY, robotRelativeSupplier),
                         () -> PoseCalculations.closeToSpeaker() || climb.getHooksUp())
                     ).finallyDo(
                         () -> 
@@ -558,20 +558,7 @@ private void configureDriverBindings(PatriBoxController controller) {
         configureCommonDriverBindings(controller);
 
         controller.leftStick()
-            .toggleOnTrue(
-                Commands.sequence(
-                    elevator.toBottomCommand(),
-                    swerve.resetHDCCommand(),
-                    limelight3g.setLEDState(() -> true),
-                    alignmentCmds.preparePassCommand(controller::getLeftX, controller::getLeftY, robotRelativeSupplier, true)
-                    ).finallyDo(
-                        () -> 
-                            limelight3g.setLEDState(() -> false)
-                            .andThen(
-                                shooterCmds.raisePivot()
-                                    .alongWith(shooterCmds.stopShooter())
-                                    .withInterruptBehavior(InterruptionBehavior.kCancelSelf))
-                            .schedule()));
+            .onTrue(shooterCmds.prepareSubwooferCommand());
 
         controller.a()
             .onTrue(swerve.resetHDCCommand())
@@ -581,10 +568,7 @@ private void configureDriverBindings(PatriBoxController controller) {
                         limelight3g.setLEDState(() -> true)))
             .onFalse(
                 limelight3g.setLEDState(() -> false));
-
-        controller.b()
-            .toggleOnTrue(shooterCmds.prepareSubwooferCommand());
-
+
         controller.x()
             .whileTrue(pieceControl.intakeNoteDriver(swerve::getPose, swerve::getRobotRelativeVelocity))
             .negate().and(() -> !OIConstants.OPERATOR_PRESENT  || !operator.getLeftBumper())

diff --git a/src/main/java/frc/robot/commands/drive/AlignmentCmds.java b/src/main/java/frc/robot/commands/drive/AlignmentCmds.java
@@ -195,10 +195,10 @@ public Command wingRotationalAlignment(DoubleSupplier driverX, DoubleSupplier dr
                 climb::getHooksUp);
     }
 
-    public Command preparePassCommand(DoubleSupplier driverX, DoubleSupplier driverY, BooleanSupplier robotRelativeSupplier, boolean lowPass) {
+    public Command preparePassCommand(DoubleSupplier driverX, DoubleSupplier driverY, BooleanSupplier robotRelativeSupplier) {
         return
             passRotationalAlignment(driverX, driverY, shooterCmds)
-                .alongWith(shooterCmds.preparePassCommand(swerve::getPose, lowPass));
+                .alongWith(shooterCmds.preparePassCommand(swerve::getPose));
     }
 
     public Command preparePresetPose(DoubleSupplier driverX, DoubleSupplier driverY, boolean xButtonPressed) {

diff --git a/src/main/java/frc/robot/commands/managers/PieceControl.java b/src/main/java/frc/robot/commands/managers/PieceControl.java
@@ -489,8 +489,7 @@ public Command getAutomaticShooterSpeeds(Supplier<Pose2d> robotPose, BooleanSupp
                                     robotPose.get().getTranslation()
                                 ).getSpeeds()
                                 : shooterCmds.shooterCalc.calculatePassTriplet(
-                                    robotPose.get(),
-                                    false
+                                    robotPose.get()
                                 ).getSpeeds()
                         ),
                     shooterCmds.getShooter()

diff --git a/src/main/java/frc/robot/commands/managers/ShooterCmds.java b/src/main/java/frc/robot/commands/managers/ShooterCmds.java
@@ -149,10 +149,10 @@ public Command prepareStillSpeakerCommand(Supplier<Pose2d> robotPose) {
         return prepareSWDCommand(robotPose, () -> new ChassisSpeeds());
     }
 
-    public Command preparePassCommand(Supplier<Pose2d> robotPose, boolean lowPass) {
+    public Command preparePassCommand(Supplier<Pose2d> robotPose) {
         return Commands.run(
             () -> {
-                desiredTriplet = shooterCalc.calculatePassTriplet(robotPose.get(), lowPass);
+                desiredTriplet = shooterCalc.calculatePassTriplet(robotPose.get());
                 pivot.setAngle(desiredTriplet.getAngle());
                 shooter.setSpeed(desiredTriplet.getSpeeds());
             }, pivot, shooter);

diff --git a/src/main/java/frc/robot/util/calc/AlignmentCalc.java b/src/main/java/frc/robot/util/calc/AlignmentCalc.java
@@ -185,7 +185,7 @@ public Supplier<ChassisSpeeds> getSourceRotationalSpeedsSupplier(DoubleSupplier
      */
     public ChassisSpeeds getSpeakerRotationalSpeeds(double driverX, double driverY, ShooterCmds shooterCmds) {
         return 
-            getRotationalSpeeds(driverX, driverY, shooterCmds.shooterCalc.calculateRobotAngleToSpeaker(swerve.getPose(), swerve.getRobotRelativeVelocity()));
+            getRotationalSpeeds(driverX, driverY, shooterCmds.shooterCalc.calculateRobotAngleToSpeaker(swerve.getPose()));
     }
 
     public ChassisSpeeds getPassRotationalSpeeds(double driverX, double driverY, ShooterCmds shooterCmds) {

diff --git a/src/main/java/frc/robot/util/calc/PoseCalculations.java b/src/main/java/frc/robot/util/calc/PoseCalculations.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import frc.robot.Robot;
 import frc.robot.RobotContainer;
 import frc.robot.util.Constants.AutoConstants;
@@ -170,4 +171,12 @@ public static boolean inSourcePassZone(Pose2d position) {
                 : position.getX() > FieldConstants.RED_WING_X;
     }
 
+    public static boolean inLowPassZone(Pose2d position) {
+        return 
+            position.getY() > FieldConstants.GET_STAGE_POINTS().get(2).getY()
+            || (Robot.isRedAlliance() 
+                ? position.getX() > FieldConstants.NOTE_TRANSLATIONS[3].getX() 
+                : position.getX() < FieldConstants.NOTE_TRANSLATIONS[0].getX());
+    }
+
 }
diff --git a/src/main/java/frc/robot/util/calc/ShooterCalc.java b/src/main/java/frc/robot/util/calc/ShooterCalc.java
@@ -84,7 +84,8 @@ public SpeedAngleTriplet calculateSWDTripletAuto(Pose2d pose, ChassisSpeeds spee
             );
     }
 
-    public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose, boolean lowPass) {
+    public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose) {
+        boolean lowPass = PoseCalculations.inLowPassZone(robotPose);
         Rotation2d pivotAngle = calculatePassPivotAngle(robotPose, lowPass);
         Pair<Number, Number> shooterSpeeds = calculateShooterSpeedsForPassApex(robotPose, pivotAngle, lowPass);
         return SpeedAngleTriplet.of(
@@ -210,7 +211,7 @@ public Rotation2d calculateRobotAngleToPose(Pose2d robotPose, ChassisSpeeds robo
             velocityTangent,
             (target.equals(FieldConstants.GET_CENTER_PASS_TARGET_POSITION()) || target.equals(FieldConstants.GET_SOURCE_PASS_TARGET_POSITION())
                 // Run this calculation with high pass triplet, as low pass requires little rotational precision in swd
-                ? rpmToVelocity(calculatePassTriplet(robotPose, false).getSpeeds())
+                ? rpmToVelocity(calculatePassTriplet(robotPose).getSpeeds())
                 : rpmToVelocity(calculateSWDTriplet(robotPose, robotVelocity).getSpeeds())
             // rpmToVelocity(calculateShooterSpeedsForApex(robotPose, calculatePivotAngle(robotPose)))
         ));
@@ -227,6 +228,10 @@ public Rotation2d calculateRobotAngleToPose(Pose2d robotPose, ChassisSpeeds robo
         return desiredRotation2d;
     }
 
+    public Rotation2d calculateRobotAngleToPose(Pose2d robotPose, Pose2d target) {
+        return calculateRobotAngleToPose(robotPose, new ChassisSpeeds(), target);
+    }
+
     public Rotation2d calculateRobotAngleToPass(Pose2d robotPose) {
         return calculateRobotAngleToPass(robotPose, new ChassisSpeeds());
     }