Updated the source to using the ROTATION_VECTOR

Since the old orientation sensor was deprecated, I've updated the code to using the new recommended method.
It's only in Step 4 at the moment.

I've also refactored the code a bit to clean it up.