Modified AngleController with new algorithm that isn't working.
May want to revert back. But need to see why this algorithm isn't working!