http://mbed.org/users/Yo_Robot/code/TrackVector2D/#958e1e10e536