"final" controller revision

Patribots4738 · Oct 11, 2024 · 4b8d745 · 4b8d745
1 parent 6e6fb2b
commit 4b8d745
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 67 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -423,23 +423,14 @@ private void configureCommonDriverBindings(PatriBoxController controller) {
         controller.start()
             .onTrue(Commands.runOnce(() -> 
                 swerve.resetOdometry(FieldConstants.GET_SUBWOOFER_POSITION()), swerve));
-
-        // Upon hitting start button
-        // reset the orientation of the robot
-        // to be facing TOWARDS the driver station
-        // TODO: for testing reset odometry to speaker
-        // controller.start()
-        //     .onTrue(Commands.runOnce(() -> 
-        //         swerve.resetOdometry(FieldConstants.GET_SUBWOOFER_POSITION().plus(new Transform2d(0,0, Rotation2d.fromDegrees(180)))), swerve
-        //     ));
-
 
         // Speaker / Source / Chain rotational alignment
         controller.rightStick()
             .onTrue(swerve.resetHDCCommand())
             .toggleOnTrue(
                 Commands.sequence(
-                    pieceControl.elevatorToBottomSafe().unless(elevator::atDesiredPosition),
+                    pieceControl.elevatorToBottomSafe()
+                        .unless(() -> elevator.getDesiredPosition() == ElevatorConstants.BOTTOM_POS),
                     limelight3g.setLEDState(() -> true),
                     new ActiveConditionalCommand(
                         // This runs SWD on heading control 
@@ -457,14 +448,6 @@ private void configureCommonDriverBindings(PatriBoxController controller) {
                                     shooterCmds.stopShooter())
                                     .withInterruptBehavior(InterruptionBehavior.kCancelSelf))
                                 .schedule()));
-
-
-        // Climbing controls
-        controller.povUp()
-            .onTrue(climb.povUpCommand());
-
-        controller.povDown()
-            .onTrue(climb.toBottomCommand().alongWith(shooterCmds.stowPivot()));
 
         // Note to target will either place amp or shoot,
         // depending on if the elevator is up or not
@@ -473,42 +456,26 @@ private void configureCommonDriverBindings(PatriBoxController controller) {
                 pieceControl.noteToTarget(
                     swerve::getPose, 
                     swerve::getRobotRelativeVelocity, 
-                    () -> swerve.atHDCAngle(), 
-                    () -> operator.getLeftBumper())
+                    swerve::atHDCAngle, 
+                    operator::getLeftBumper)
                     .alongWith(driver.setRumble(() -> 0.5, 0.3))
             .andThen(Commands.runOnce(() -> RobotContainer.hasPiece = false)));
-
-
-        // Intake controls 
-        // The warning of dead code only applies if we are using single driver mode
-        // controller.leftBumper()
-        //     .whileTrue(pieceControl.intakeNoteDriver(swerve::getPose, swerve::getRobotRelativeVelocity))
-        //     .negate().and(() -> !OIConstants.OPERATOR_PRESENT  || !operator.getLeftBumper())
-        //     .onTrue(pieceControl.stopIntakeAndIndexer());
 
         controller.rightBumper()
             .onTrue(pieceControl.ejectNote())
             .negate().and(() -> !OIConstants.OPERATOR_PRESENT  || !operator.getRightBumper())
             .onTrue(pieceControl.stopEjecting());
+
+        // Climbing controls
+        controller.povUp()
+            .onTrue(climb.povUpCommand());
+
+        controller.povDown()
+            .onTrue(climb.toBottomCommand().alongWith(shooterCmds.stowPivot()));
 
         // POV left is uncommonly used but needed incase of emergency
         controller.povLeft()
             .onTrue(pieceControl.stopAllMotors().andThen(shooterCmds.raisePivot()));
-
-        // Cat smirk
-        // controller.povRight()
-        //     .toggleOnTrue(Commands.sequence(
-        //             elevator.toBottomCommand(),
-        //             swerve.resetHDCCommand(),
-        //             limelight3g.setLEDState(() -> true),
-        //             alignmentCmds.moveAndPreparePresetCommand()
-        //                 .finallyDo(
-        //                     () -> 
-        //                         limelight3g.setLEDState(() -> false)
-        //                         .andThen(shooterCmds.raisePivot()
-        //                             .alongWith(shooterCmds.stopShooter())
-        //                             .withInterruptBehavior(InterruptionBehavior.kCancelSelf))
-        //                         .schedule())));
 
     }
 
@@ -562,22 +529,23 @@ private void configureDriverBindings(PatriBoxController controller) {
 
         controller.leftStick()
             .toggleOnTrue(
-                shooterCmds.prepareSubwooferCommand()
-                    .finallyDo(
-                        () -> 
-                        Commands.parallel(
-                            shooterCmds.raisePivot(),
-                            shooterCmds.stopShooter())
-                            .withInterruptBehavior(InterruptionBehavior.kCancelSelf)
-                            .schedule()));
+                Commands.sequence(
+                    pieceControl.elevatorToBottomSafe(),
+                    shooterCmds.prepareSubwooferCommand()
+                ).finallyDo(
+                    () -> 
+                    Commands.parallel(
+                        shooterCmds.raisePivot(),
+                        shooterCmds.stopShooter())
+                        .withInterruptBehavior(InterruptionBehavior.kCancelSelf)
+                        .schedule()));
 
 
         controller.a()
             .onTrue(swerve.resetHDCCommand())
             .whileTrue(
                 alignmentCmds.ampRotationalAlignmentCommand(controller::getLeftX, controller::getLeftY)
-                    .alongWith(
-                        limelight3g.setLEDState(() -> true)))
+                    .alongWith(limelight3g.setLEDState(() -> true)))
             .onFalse(
                 Commands.parallel(
                     limelight3g.setLEDState(() -> false),

diff --git a/src/main/java/frc/robot/commands/managers/PieceControl.java b/src/main/java/frc/robot/commands/managers/PieceControl.java
@@ -377,19 +377,20 @@ public Command elevatorToPlacement(boolean povLeftPosition) {
                 Commands.either(
                     elevatorToNoteFixSafe(), 
                     elevatorToTopSafe(), 
-                    () -> povLeftPosition),
+                    () -> povLeftPosition), 
                 placeWhenReady().onlyIf(this::shouldPlaceWhenReady));
     }
 
     public Command placeWhenReady() {
         return
             new SelectiveConditionalCommand(
-                ampper.outtake(NT.getValue("placeOuttake"))
-                    .andThen(
-                        elevatorToBottom()
-                        .alongWith(shooterCmds.raisePivot())
-                        .alongWith(ampper.stopCommand())
-                    ),
+                Commands.sequence(
+                    ampper.outtake(NT.getValue("placeOuttake")),
+                    ampper.stopCommand(),
+                    setPlaceWhenReadyCommand(false),
+                    elevatorToBottomSafe(),
+                    shooterCmds.raisePivot()
+                ),
                 setPlaceWhenReadyCommand(true),
                 elevator::atDesiredPosition);
     }
@@ -448,7 +449,7 @@ public Command moveNote(boolean desiredSide) {
                 Commands.runOnce(() -> this.desiredSide = desiredSide),
                 setCurrentlyMovingNote(true),
                 Commands.either(
-                    noteToIndexer(),
+                    intakeUntilNote(),
                     noteToTrap(),
                     () -> desiredSide))
                 .finallyDo(() -> {

diff --git a/src/main/java/frc/robot/commands/managers/ShooterCmds.java b/src/main/java/frc/robot/commands/managers/ShooterCmds.java
@@ -201,11 +201,7 @@ public Command stowPivot() {
     public Command raisePivot() {
         return pivot.setAngleCommand(ShooterConstants.PIVOT_RAISE_ANGLE_DEGREES);
 	}
-
-    public Command togglePivotLock() {
-        return pivot.togglePivotLockCommand();
-    }
-
+
     public Command lockPivot() {
         return pivot.setPivotLockCommand(true);
     }