ControlBoard portion of Team254's Robot

Robot(Snippet).java

 /**
     * This function is called periodically during operator control.
     *
     * The code uses state machines to ensure that no matter what buttons the driver presses, the robot behaves in a
     * safe and consistent manner.
     *
     * Based on driver input, the code sets a desired state for each subsystem. Each subsystem will constantly compare
     * its desired and actual states and act to bring the two closer.
     */
   public void teleopPeriodic() {
        try {
            double timestamp = Timer.getFPGATimestamp();
            // Drive base
            double throttle = mControlBoard.getThrottle();
            double turn = mControlBoard.getTurn();

            boolean wants_aim_button = mControlBoard.getAimButton();
            // wants_aim_button = !mDelayedAimButton.update(timestamp, !wants_aim_button);

            if (wants_aim_button || mControlBoard.getDriveAimButton()) {

                if (Constants.kIsShooterTuning) {
                    mDrive.setWantAimToGoal();
                } else if (mControlBoard.getDriveAimButton()) {
                    mDrive.setWantDriveTowardsGoal();
                } else {
                    mDrive.setWantAimToGoal();
                }

                if ((mControlBoard.getDriveAimButton() && !mDrive.isApproaching())
                        || !mControlBoard.getDriveAimButton()) {
                    if (mControlBoard.getUnjamButton()) {
                        mSuperstructure.setWantedState(Superstructure.WantedState.UNJAM_SHOOT);
                    } else {
                        mSuperstructure.setWantedState(Superstructure.WantedState.SHOOT);
                    }
                } else {
                    mSuperstructure.setWantedState(Superstructure.WantedState.RANGE_FINDING);
                }
            } else {
                // Make sure not to interrupt shooting spindown.
                if (!mSuperstructure.isShooting()) {
                    mDrive.setOpenLoop(mCheesyDriveHelper.cheesyDrive(throttle, turn, mControlBoard.getQuickTurn(),
                            !mControlBoard.getLowGear()));
                    boolean wantLowGear = mControlBoard.getLowGear();
                    mDrive.setHighGear(!wantLowGear);
                }

                Intake.getInstance().setCurrentThrottle(mControlBoard.getThrottle());

                boolean wantsExhaust = mControlBoard.getExhaustButton();

                if (Constants.kIsShooterTuning) {
                    mLED.setWantedState(LED.WantedState.FIND_RANGE);
                    if (mCommitTuning.update(mControlBoard.getLowGear())) {
                        // Commit to TuningMap.
                        double rpm = mSuperstructure.getCurrentTuningRpm();
                        double range = mSuperstructure.getCurrentRange();
                        System.out.println("Tuning range: " + range + " = " + rpm);
                        mTuningFlywheelMap.put(new InterpolatingDouble(range), new InterpolatingDouble(rpm));
                        mSuperstructure.incrementTuningRpm();
                    }
                }

                // Exhaust has highest priority for intake.
                if (wantsExhaust) {
                    mSuperstructure.setWantIntakeReversed();
                } else if (mControlBoard.getIntakeButton()) {
                    mSuperstructure.setWantIntakeOn();
                } else if (!mSuperstructure.isShooting()) {
                    mSuperstructure.setWantIntakeStopped();
                }

                // Hanging has highest priority for feeder, followed by exhausting, unjamming, and finally
                // feeding.
                if (mControlBoard.getHangButton()) {
                    mSuperstructure.setWantedState(Superstructure.WantedState.HANG);
                } else if (wantsExhaust) {
                    mSuperstructure.setWantedState(Superstructure.WantedState.EXHAUST);
                } else if (mControlBoard.getUnjamButton()) {
                    mSuperstructure.setWantedState(Superstructure.WantedState.UNJAM);
                } else if (mControlBoard.getFeedButton()) {
                    mSuperstructure.setWantedState(Superstructure.WantedState.MANUAL_FEED);
                } else if (mControlBoard.getRangeFinderButton()) {
                    mSuperstructure.setWantedState(Superstructure.WantedState.RANGE_FINDING);
                } else {
                    mSuperstructure.setWantedState(Superstructure.WantedState.IDLE);
                }

                if (mControlBoard.getFlywheelSwitch()) {
                    mSuperstructure.setClosedLoopRpm(Constants.kDefaultShootingRPM);
                } else if (mControlBoard.getShooterOpenLoopButton()) {
                    mSuperstructure.setShooterOpenLoop(0);
                } else if (mControlBoard.getShooterClosedLoopButton()) {
                    mSuperstructure.setClosedLoopRpm(Constants.kDefaultShootingRPM);
                } else if (!mControlBoard.getHangButton() && !mControlBoard.getRangeFinderButton() &&
                        !mSuperstructure.isShooting()) {
                    mSuperstructure.setShooterOpenLoop(0);
                }

                if (mControlBoard.getBlinkLEDButton()) {
                    mLED.setWantedState(LED.WantedState.BLINK);
                }
            }

            boolean score_gear = mControlBoard.getScoreGearButton();
            boolean grab_gear = mControlBoard.getGrabGearButton();

            if (score_gear && grab_gear) {
                mGearGrabber.setWantedState(WantedState.CLEAR_BALLS);
            } else if (score_gear) {
                mGearGrabber.setWantedState(WantedState.SCORE);
            } else if (grab_gear) {
                mGearGrabber.setWantedState(WantedState.ACQUIRE);
            } else {
                mGearGrabber.setWantedState(WantedState.IDLE);
            }

            mSuperstructure.setActuateHopper(mControlBoard.getActuateHopperButton());
            allPeriodic();
        } catch (Throwable t) {
            CrashTracker.logThrowableCrash(t);
            throw t;
        }
    }