Added Robot Angle Calculation to Robot Position Calculation.
Angle Calculation not quite right -.- :(