Code to control a xyz table robot w.r.t. an origin position