[wireties]

When you are not directly in front of the goal you are tracking a trapezoid, not a rectangle. I have not looked at the code, does it account for this? Perhaps you can feed a rectangle into your code and use the height of the left or right side of your trapezoid. BTW, the relative heights of the left and right side will tell you want angle you have.

HTH