Upgraded rotate to heading function. (Simple P Controller)