Added functions to convert from Quaternion to Euler angles and initial
support for self driving to desired position. Very alpha.