Robo coordinates and theta calcs

What we had in matlab, now we can do in C!