Fixed compile error on StandardFirmata example.

Revert "Servo library refactoring. See #1617"

This reverts commit e8834d32f3f09cf6ce9a52bd30ba552bcb8291ef.

See #1617