Trying to make algorithm for managing reprap movement