Updated encoder code

Added averaging to smooth angle value, and it works over the 360 to 0
gap