Fixed bug detecting when PID motions are finished