fix typo in 2nd xy2Xtheta calculation

Minus instead of plus meant it would never swing around.