Add and use wait_ms(); fix some timings