Debugged unlock pawl routine and setting servo's based on velocity