There is too much copy/pasted code to control servos. I should make a library to clean all of it up, and use that library here.