From 377000493efd4ad904e8ef4f4ed4983fb27ff11e Mon Sep 17 00:00:00 2001 From: Max Nargang Date: Wed, 22 May 2024 17:33:07 -0400 Subject: [PATCH] Make all of the MechanismCommands trigger haptics Groundwork for moving the haptics enable/disable logic out of MechanismCommands --- src/main/java/frc/robot/RobotContainer.java | 20 ++-- .../frc/robot/commands/MechanismCommands.java | 96 ++++++------------- 2 files changed, 39 insertions(+), 77 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4c44475..7e492e6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -188,8 +188,8 @@ private void configureDriverBindings(){ //TODO: Shoot should not need the position passed in //TODO: Rename DBOT to MID_STAGE to be more descriptive - driverControllerCommands.a().onTrue(mechanismCommands.PrepareShootWithHaptics(ShootingPosition.DBOT)) - .onFalse(mechanismCommands.ShootWithHaptics()); + driverControllerCommands.a().onTrue(mechanismCommands.PrepareShoot(ShootingPosition.DBOT)) + .onFalse(mechanismCommands.Shoot()); //TODO: This can be turned into a drive to source function /*driverControllerCommands.b().onTrue(drivetrain.driveToPose( @@ -201,19 +201,19 @@ private void configureDriverBindings(){ private void configureOperatorBindings(){ operatorControllerCommands.x().and(() -> !isClimbMode).onTrue(arm.Stow()); operatorControllerCommands.y().and(() -> !isClimbMode).whileTrue(head.StartOutake()).onFalse(head.StopIntake()); - operatorControllerCommands.a().and(() -> !isClimbMode).onTrue(mechanismCommands.IntakeGroundWithHaptics().andThen(arm.Stow())); - operatorControllerCommands.b().and(() -> !isClimbMode).onTrue(mechanismCommands.IntakeSourceWithHaptics()); + operatorControllerCommands.a().and(() -> !isClimbMode).onTrue(mechanismCommands.IntakeGround(true).andThen(arm.Stow())); + operatorControllerCommands.b().and(() -> !isClimbMode).onTrue(mechanismCommands.IntakeSource()); - operatorControllerCommands.leftTrigger().onTrue(mechanismCommands.PrepareShootWithHaptics(ShootingPosition.AMP)); - operatorControllerCommands.leftBumper().onTrue(mechanismCommands.ShootWithHaptics()); + operatorControllerCommands.leftTrigger().onTrue(mechanismCommands.PrepareShoot(ShootingPosition.AMP)); + operatorControllerCommands.leftBumper().onTrue(mechanismCommands.Shoot()); - operatorControllerCommands.rightTrigger().onTrue(mechanismCommands.PrepareShootWithHaptics(drivetrain::getDistanceToSpeaker)) - .onFalse(mechanismCommands.PrepareShootWithHaptics(drivetrain::getDistanceToSpeaker).andThen(mechanismCommands.Shoot()).andThen(arm.Stow())); + operatorControllerCommands.rightTrigger().onTrue(mechanismCommands.PrepareShoot(drivetrain::getDistanceToSpeaker)) + .onFalse(mechanismCommands.PrepareShoot(drivetrain::getDistanceToSpeaker).andThen(mechanismCommands.Shoot()).andThen(arm.Stow())); - operatorControllerCommands.rightBumper().onTrue(mechanismCommands.PrepareShootWithHaptics(ShootingPosition.SUBWOOFER)).onFalse(mechanismCommands.ShootWithHaptics()); + operatorControllerCommands.rightBumper().onTrue(mechanismCommands.PrepareShoot(ShootingPosition.SUBWOOFER)).onFalse(mechanismCommands.Shoot()); operatorControllerCommands.povLeft().onTrue(head.StopIntake().andThen(head.SpinDownShooter())); - operatorControllerCommands.povRight().onTrue(mechanismCommands.PrepareShootWithHaptics(ShootingPosition.PODIUM)).onFalse(mechanismCommands.ShootWithHaptics()); + operatorControllerCommands.povRight().onTrue(mechanismCommands.PrepareShoot(ShootingPosition.PODIUM)).onFalse(mechanismCommands.Shoot()); operatorControllerCommands.povUp().onTrue(Commands.runOnce(() -> climber.setSpeed(ClimberConstants.CLIMB_RATE, ClimberConstants.CLIMB_RATE), climber)).onFalse(Commands.runOnce(() -> climber.setSpeed(0, 0), climber)); diff --git a/src/main/java/frc/robot/commands/MechanismCommands.java b/src/main/java/frc/robot/commands/MechanismCommands.java index e775813..c2894d1 100644 --- a/src/main/java/frc/robot/commands/MechanismCommands.java +++ b/src/main/java/frc/robot/commands/MechanismCommands.java @@ -41,7 +41,8 @@ public MechanismCommands(Arm arm, Head head, HapticController driverController, } /** - * will finish after piece has been intaken + * Intake from the source + * Finishes after piece has been intaken * * @return {@link Command} */ @@ -49,46 +50,30 @@ public Command IntakeSource() { return head.SpinDownShooter() .andThen(() -> arm.setArmTarget(ArmConstants.SOURCE_ANGLE)) .andThen(() -> arm.setElevatorTarget(ElevatorConstants.MAX_HEIGHT)) - .andThen(head.IntakePiece()); - } - - /** - * Intakes from the source and triggers haptics once done. - * will finish after piece has been intaken - * - * @return {@link Command} - */ - public Command IntakeSourceWithHaptics() { - return IntakeSource() + .andThen(head.IntakePiece()) .andThen(new ScheduleCommand(driverController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))) .andThen(new ScheduleCommand(operatorController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))); } /** - * will finish after piece has been intaken + * Intake from the ground + * Finishes after piece has been intaken * + * @param stopShooter + * should the shooter be stopped before intaking? * @return {@link Command} */ public Command IntakeGround(boolean stopShooter) { return Commands.either(head.SpinDownShooter(), Commands.none(), () -> stopShooter) .andThen(() -> arm.setArmTarget(ArmConstants.FLOOR_PICKUP)) .andThen(() -> arm.setElevatorTarget(ElevatorConstants.MAX_HEIGHT)) - .andThen(head.IntakePiece()); - } - - /** - * will finish after piece has been intaken - * - * @return {@link Command} - */ - public Command IntakeGroundWithHaptics() { - return IntakeGround(true) + .andThen(head.IntakePiece()) .andThen(new ScheduleCommand(driverController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))) .andThen(new ScheduleCommand(operatorController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))); } /** - * cancel shoot and intake and stow + * Cancel shoot, intake, and stow * * @return {@link Command} */ @@ -99,15 +84,15 @@ public Command StowStopIntakeAndShooter() { } // THIS ISNT CODE DUPLICATION IT DOES A FUNDAMENTALLY DIFFERENT THING!!!!! - // TODO: I see that this is different, but when would it be used? Should be renamed to better describe - // what it does. Why doesn't it stop the shooter? Will it be used for auto? + // TODO: I see that this is different, but when would it be used? Should be renamed to better describe what it does. Why doesn't it stop the shooter? Will it be used for auto? public Command AutoStowAndStopIntake() { return arm.Stow() .andThen(head.StopIntake()); } /** - * will finish when ready + * Prepare to shoot from a {@link ShootingPosition} + * Finishes when ready * * @param position * position to shoot from @@ -115,11 +100,13 @@ public Command AutoStowAndStopIntake() { */ public Command PrepareShoot(ShootingPosition position) { return arm.SetTargets(position) - .andThen(head.SpinUpShooter(position)); + .andThen(head.SpinUpShooter(position)) + .andThen(new ScheduleCommand(operatorController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))); } /** - * will finish when ready + * Prepare to shoot from a distance + * Finishes when ready * * @param distance * distance to shoot from @@ -127,7 +114,8 @@ public Command PrepareShoot(ShootingPosition position) { */ public Command PrepareShoot(Supplier distance) { return arm.SetTargets(distance) - .andThen(head.SpinUpShooter(ShootingConstants.VARIABLE_DISTANCE_SHOT)); + .andThen(head.SpinUpShooter(ShootingConstants.VARIABLE_DISTANCE_SHOT)) + .andThen(new ScheduleCommand(operatorController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))); } public Command AutonomousPrepareShoot(Supplier distance) { @@ -136,43 +124,28 @@ public Command AutonomousPrepareShoot(Supplier distance) { } /** - * Prepare to shoot and trigger haptics when ready. - * will finish when ready + * Shoot a note + * Finishes after piece has been shot * - * @param position - * position to shoot from * @return {@link Command} */ - public Command PrepareShootWithHaptics(ShootingPosition position) { - return PrepareShoot(position) - .andThen(new ScheduleCommand(operatorController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))); - } - - /** - * Prepare to shoot and trigger haptics when ready. - * will finish when ready - * - * @param distance - * distance to shoot from - * @return {@link Command} - */ - public Command PrepareShootWithHaptics(Supplier distance) { - return PrepareShoot(distance) - .andThen(new ScheduleCommand(operatorController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))); + public Command Shoot() { + return Shoot(true); } /** - * will finish after piece has been shot + * Shoot a note + * Finishes after piece has been shot * + * @param stopShooter + * should the shooter be stopped after shooting? * @return {@link Command} */ - public Command Shoot() { - return Shoot(true); - } - public Command Shoot(boolean stopShooter) { return Commands.waitUntil(() -> arm.areArmAndElevatorAtTarget()) - .andThen(head.Shoot(stopShooter)); + .andThen(head.Shoot(stopShooter)) + .andThen(new ScheduleCommand(driverController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))) + .andThen(new ScheduleCommand(operatorController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))); } public Command AutonomousShoot(ShootingPosition position) { @@ -184,15 +157,4 @@ public Command AutonomousShoot(Drivetrain drivetrain) { return AutonomousPrepareShoot(() -> drivetrain.getDistanceToSpeaker()) .andThen(Shoot(false)); } - - /** - * will finish after piece has been shot - * - * @return {@link Command} - */ - public Command ShootWithHaptics() { - return Shoot() - .andThen(new ScheduleCommand(driverController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))) - .andThen(new ScheduleCommand(operatorController.HapticTap(RumbleType.kBothRumble, 0.3, 0.3))); - } }