Added PositionController class.
added javadoc and few bugfixes
Merge pull request #1 from XiDevelopment/angle

Added Anglecalculation Controller