Added angle calculations to distance node piniata
Merge https://github.com/robotics-silver-surfer/surfer-main