Added calculateServoPos for MODE_COMBINED. Maaayyyybbeeee it will work???