X,Y Position Calculation Added

-Added position calculation for Y
-New algorithrm used for postion calculation

Bugs:
-Encoder may not give correct results if reading interval is too small.