Motor control waits

Added criteria to ensure motors reach position before resuming loop.
Added delay between cycles.