fix servo bug