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

This reverts commit 52e75a3bdb4c8d1d9a170d29783715bc545f50dd.