Added Euler2Quaternion function
Added some logic to PID controller