[Brian Selle]

Originally Posted by tomy

So you would do something like this with more sensors?

Do you intend to use the sensors independently or coupled together? For example, would you like a drive-to-distance-PID that uses encoders for distance + a gyro to help go straight? Or perhaps you are looking to drive straight until the ultrasonic sensor trips? Some of these cases are easily handled by the default WPILib PID implementation, others not so much.

For theses cases, we typically use a separate control loop thread and switch between modes. The update method might look something like this:

	public void controlLoopUpdate() {
		if (controlMode == DriveTrainControlMode.JOYSTICK) {
			driveWithJoystick();
		}
		else if (!isFinished) {
			if (controlMode == DriveTrainControlMode.MP_STRAIGHT) {
				isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveTrainControlMode.MP_TURN) {
				isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveTrainControlMode.PID_TURN) {
				isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); 
			}
		}
	}

For more detail, you can look at the Drivetrain subsystem in our 2016 code: https://github.com/Team3310/FRC-2016