Fixing RK05 servo chatter