Fixes issues with servo write and new servo implementation.