Math update

I have not tested this yet, but this is mainly the math code for having
the arm go to points on the plate.