Fixed position calculation. Added wrapAngle.