Skip to content

Commit

Permalink
game piece collected rumble (#128)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Alenguye582
Alenguye582 authored Oct 26, 2024
1 parent 72d4fa4 commit 9968f01
Show file tree
Hide file tree
Showing 5 changed files with 29 additions and 23 deletions.
15 changes: 9 additions & 6 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,9 @@ public static class constControllers {

public static final double DRIVER_RUMBLE = 0.5;
public static final double OPERATOR_RUMBLE = 0.5;

public static final double DRIVER_GP_COLLECTED_RUMBLE = 0.3;
public static final double OPERATOR_GP_COLLECTED_RUMBLE = 0.3;
}

public static class constDrivetrain {
Expand Down Expand Up @@ -204,7 +207,7 @@ private static final class blueConstants {
private static final Pose3d SUBWOOFER = new Pose3d(new Pose2d(1.35, 5.50, Rotation2d.fromDegrees(0)));

private static final Pose3d SHUFFLE = new Pose3d(
new Pose2d(1.2991523742675781, 7.103456497192383, Rotation2d.fromDegrees(0)));
new Pose2d(3.42, 6.08, Rotation2d.fromDegrees(0)));
}

private static final class redConstants {
Expand All @@ -231,7 +234,7 @@ private static final class redConstants {
private static final Pose3d SUBWOOFER = new Pose3d(
new Pose2d(FIELD_LENGTH.in(Units.Meters) - 1.35, 5.50, Rotation2d.fromDegrees(180)));
private static final Pose3d SHUFFLE = new Pose3d(
new Pose2d(FIELD_LENGTH.in(Units.Meters) - 1.2991523742675781, 7.103456497192383, Rotation2d.fromDegrees(0)));
new Pose2d(FIELD_LENGTH.in(Units.Meters) - 3.42, 6.08, Rotation2d.fromDegrees(0)));
}
}

Expand Down Expand Up @@ -379,16 +382,16 @@ public ShooterPositionGroup(Measure<Angle> shooterAngle, Measure<Velocity<Angle>
}

public static final ShooterPositionGroup PREP_NONE = new ShooterPositionGroup(NONE_STATE_ANGLE,
Units.RotationsPerSecond.of(35), Units.RotationsPerSecond.of(35), Units.Meters.of(0));
Units.RotationsPerSecond.of(10), Units.RotationsPerSecond.of(10), Units.Meters.of(0));
public static final ShooterPositionGroup PREP_AMP_SHOOTER = new ShooterPositionGroup(Units.Degrees.of(111),
Units.RotationsPerSecond.of(10), Units.RotationsPerSecond.of(10), Units.Meters.of(0));
// Amping w/ amper
public static final ShooterPositionGroup PREP_AMP = new ShooterPositionGroup(Units.Degrees.of(99),
Units.RotationsPerSecond.of(10), Units.RotationsPerSecond.of(10), Units.Meters.of(0.46));
public static final ShooterPositionGroup PREP_SUB_BACKWARDS = new ShooterPositionGroup(Units.Degrees.of(111),
Units.RotationsPerSecond.of(35), Units.RotationsPerSecond.of(35), Units.Meters.of(0.46));
public static final ShooterPositionGroup PREP_SHUFFLE = new ShooterPositionGroup(Units.Degrees.of(42),
Units.RotationsPerSecond.of(35), Units.RotationsPerSecond.of(35), Units.Meters.of(0));
public static final ShooterPositionGroup PREP_SHUFFLE = new ShooterPositionGroup(Units.Degrees.of(34),
Units.RotationsPerSecond.of(30), Units.RotationsPerSecond.of(30), Units.Meters.of(0));
public static final ShooterPositionGroup PREP_SUB = new ShooterPositionGroup(Units.Degrees.of(45),
Units.RotationsPerSecond.of(35), Units.RotationsPerSecond.of(35), Units.Meters.of(0));
public static final ShooterPositionGroup PREP_SPIKE = new ShooterPositionGroup(Units.Degrees.of(27),
Expand Down Expand Up @@ -650,7 +653,7 @@ public static class constLEDs {
public static final int LED_STRIP_START_INDEX = 8;

public static final int[] CLEAR_LEDS = { 0, 0, 0 };
public static final int[] INTAKING_COLOR = { 0, 0, 0 };
public static final int[] GAME_PIECE_COLLECTED_COLOR = { 0, 0, 255 };
public static final int[] PREP_AMP_COLOR = { 200, 0, 255 };
public static final int[] PREP_SUB_BACKWARDS_COLOR = { 255, 255, 0 };
public static final int[] PREP_SPEAKER_COLOR = { 255, 130, 0 };
Expand Down
29 changes: 14 additions & 15 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,11 @@ public class RobotContainer {
private final static Transfer subTransfer = new Transfer();
private final static Limelight subLimelight = new Limelight();

private final Trigger gamePieceTrigger = new Trigger(() -> subTransfer.getGamePieceStored());
private final Trigger gamePieceStoredTrigger = new Trigger(() -> subTransfer.getGamePieceStored());
private final Trigger gamePieceCollectedTrigger = new Trigger(() -> subIntake.getGamePieceCollected());

private final BooleanSupplier readyToShootOperator = (() -> subDrivetrain.isDrivetrainFacingSpeaker()
private final BooleanSupplier readyToShootOperator = (() -> (subDrivetrain.isDrivetrainFacingSpeaker()
|| subDrivetrain.isDrivetrainFacingShuffle())
&& subShooter.readyToShoot() && subStateMachine.isCurrentStateTargetState()
&& subTransfer.getGamePieceStored());

Expand All @@ -94,7 +96,7 @@ public RobotContainer() {
new Trigger(() -> conDriver.btn_X.getAsBoolean() || conDriver.btn_LeftTrigger.getAsBoolean())));

// - Manual Triggers -
gamePieceTrigger
gamePieceStoredTrigger
.onTrue(Commands
.deferredProxy(
() -> subStateMachine.tryState(RobotState.STORE_FEEDER, subStateMachine, subClimber, subDrivetrain,
Expand All @@ -104,15 +106,12 @@ public RobotContainer() {
subElevator, subDrivetrain))))
.onTrue(new GamePieceRumble(conDriver, conOperator).asProxy());

// new Trigger(readyToShootOperator).onTrue(
// Commands.runOnce(() -> conDriver.setRumble(RumbleType.kBothRumble,
// constControllers.DRIVER_RUMBLE)).alongWith(
// Commands.runOnce(() -> conOperator.setRumble(RumbleType.kBothRumble,
// constControllers.OPERATOR_RUMBLE))))
// .onFalse(
// Commands.runOnce(() -> conDriver.setRumble(RumbleType.kBothRumble,
// 0)).alongWith(
// Commands.runOnce(() -> conOperator.setRumble(RumbleType.kBothRumble, 0))));
gamePieceCollectedTrigger
.onTrue(Commands
.runOnce(() -> conDriver.setRumble(RumbleType.kLeftRumble, constControllers.DRIVER_GP_COLLECTED_RUMBLE)))
.onTrue(Commands.runOnce(
() -> conOperator.setRumble(RumbleType.kLeftRumble, constControllers.OPERATOR_GP_COLLECTED_RUMBLE)))
.onTrue(Commands.runOnce(() -> subLEDs.setLEDs(constLEDs.GAME_PIECE_COLLECTED_COLOR)));

new Trigger(readyToShootOperator).onTrue(
Commands.runOnce(() -> conOperator.setRumble(RumbleType.kBothRumble,
Expand All @@ -138,7 +137,7 @@ public RobotContainer() {
NamedCommands.registerCommand("Intaking", Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter))
.until(gamePieceTrigger));
.until(gamePieceStoredTrigger));

SmartDashboard.putNumber("Preload Only Delay", 0);

Expand Down Expand Up @@ -172,7 +171,7 @@ private void configureOperatorBindings(SN_XboxController controller) {
.onFalse(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter))
.unless(gamePieceTrigger));
.unless(gamePieceStoredTrigger));

// Shoot
controller.btn_RightTrigger.whileTrue(
Expand All @@ -181,7 +180,7 @@ private void configureOperatorBindings(SN_XboxController controller) {
.onFalse(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter))
.unless(gamePieceTrigger));
.unless(gamePieceStoredTrigger));

// Prep with vision
controller.btn_RightBumper.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)))
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/subsystems/Drivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,6 +169,10 @@ public boolean isDrivetrainFacingSpeaker() {
return isDrivetrainAtAngle(getAngleToSpeaker());
}

public boolean isDrivetrainFacingShuffle() {
return isDrivetrainAtAngle(getAngleToShuffle());
}

/**
* Calculates the angle necessary for the shooter to face a given coordinate.
*
Expand Down Expand Up @@ -279,5 +283,6 @@ public void periodic() {

SmartDashboard.putNumber("Drivetrain Rotation", getRotation().getDegrees());
SmartDashboard.putBoolean("Drivetrain Facing Speaker", isDrivetrainFacingSpeaker());
SmartDashboard.putBoolean("Drivetrain Facing Shuffle", isDrivetrainFacingShuffle());
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -385,9 +385,9 @@ public void periodic() {

SmartDashboard.putBoolean("Shooter/Safe to Move Elevator", isSafeToMoveElevator());
SmartDashboard.putBoolean("Shooter/Ready to Shoot", readyToShoot());
SmartDashboard.putNumber("Shooter/Last Desired Pivot Angle", lastDesiredPivotAngle.in(Units.Degrees));

SmartDashboard.putBoolean("Zeroing/Pivot/Attempting Zeroing", attemptingZeroing);
SmartDashboard.putBoolean("Zeroing/Pivot/Has Zeroed", hasZeroed);

}
}
1 change: 0 additions & 1 deletion src/main/java/frc/robot/subsystems/StateMachine.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -382,6 +382,5 @@ public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putString("CURRENT ROBOT STATE", getRobotState().toString());
SmartDashboard.putString("CURRENT TARGET STATE", getTargetState().toString());

}
}

0 comments on commit 9968f01

Please sign in to comment.