Refactored servo hookup code to try to simplify it some. Still kind of gross.