Skip to content

Commit

Permalink
Make all of the MechanismCommands trigger haptics
Browse files Browse the repository at this point in the history 
Groundwork for moving the haptics enable/disable logic out of MechanismCommands
  • Loading branch information
@CoffeeCoder1
CoffeeCoder1 committed May 22, 2024
1 parent 03413e5 commit 3770004
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 77 deletions.
20 changes: 10 additions & 10 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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));
Expand Down
96 changes: 29 additions & 67 deletions src/main/java/frc/robot/commands/MechanismCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,54 +41,39 @@ 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}
*/
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}
*/
Expand All @@ -99,35 +84,38 @@ 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
* @return {@link Command}
*/
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
* @return {@link Command}
*/
public Command PrepareShoot(Supplier<Double> 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<Double> distance) {
Expand All @@ -136,43 +124,28 @@ public Command AutonomousPrepareShoot(Supplier<Double> 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<Double> 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) {
Expand All @@ -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)));
}
}

0 comments on commit 3770004

Please sign in to comment.