XY refactoring

XY refactoring for getting local coordinates of the XY controller and
normalization.