Add a ms delay macro and use in wait loops to avoid BSY timeouts