Changing between states

Thanks for your reply.
I found the issue to be a combination of not returning the device to idle properly before changing modes, and that the distance calculation added a little bit of delay when device B was transmitting A was not listening.