diff --git a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java index 396ca93..b448339 100644 --- a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java +++ b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java @@ -109,8 +109,9 @@ public ControlBoard(RobotContainer robot) { new Trigger(driver.rightStickButton::isPressed) .debounce(0.1) .whileTrue(Commands.run(() -> robot.drive.turn(new TurnRequest(SwerveDrive.DRIVER_PRIORITY + 1, new Rotation2d(11.0))))); - // Just angle amp - new Trigger(() -> driver.a.isPressed()).whileTrue(new AmpAlignCommand(robot.drive).alongWith()); // FIXME: Make the bar go up too + + // Angle towards the amp + new Trigger(() -> driver.a.isPressed()).whileTrue(new AmpAlignCommand(robot.drive)); driver.y.onFalling(() -> NTData.SHOOTER_PIVOT_RECALIBRATE.set(true)); @@ -121,13 +122,14 @@ public ControlBoard(RobotContainer robot) { new Trigger(() -> robot.indexer.hasPiece() || !operator.a.isPressed()) .onTrue(Commands.runOnce(() -> robot.intake.set(IntakeSubsystem.State.OFF))); - new Trigger(() -> CHARACTERISE_WHEEL_RADIUS.get()).whileTrue(new CharactarizeWheelCommand(robot.drive)); forceSubwooferTrigger = new Trigger(() -> driver.dpad.down.isPressed()).debounce(0.1); new Trigger(operator.start::isPressed) .onTrue(Commands.runOnce(robot.indexer::beginReverse)) .onFalse(Commands.runOnce(robot.indexer::endReverse)); + + new Trigger(() -> CHARACTERISE_WHEEL_RADIUS.get()).whileTrue(new CharactarizeWheelCommand(robot.drive)); } private boolean driverWantsAim() { @@ -171,11 +173,6 @@ private double getDriveRotation() { return -squareWithSign(driver.rightStickX.get()) * NTData.TURN_SPEED.get(); } - private boolean getRobotRelativeDrive() { - // return driverRobotRelDebounce.calculate(driver.leftBumper.isPressed()); - return false; - } - private enum ClimbState { IDLE(ClimberArm.State.RETRACTED, null), EXTENDED(ClimberArm.State.EXTENDED, AmpArm2Subsystem.Position.CLIMB_OUT_OF_THE_WAY), @@ -246,10 +243,6 @@ public void periodic() { rotation.getRadians(), robot.drive.getEstimatedPose().getRotation()); - if (getRobotRelativeDrive()) { - chassisRequest = new ChassisSpeeds(-translation.getX(), -translation.getY(), rotation.getRadians()); - } - robot.drive.driveAndTurn( SwerveDrive.DRIVER_PRIORITY, chassisRequest,