Removed the 5 addition to the angle. Angles are still not accurate. Need to do some calibration or come up with fudge factors.