Adding goniometer motors initialization/control