updated and tested ServoFirmata example