Adds SERVO mode

Signed-off-by: Rick Waldron <waldron.rick@gmail.com>