Fix the servo logic