New arm software in progress

Got angle conversions working.
Playing with the zero switch. Logic still buggy for that.
Will start on testing vision as soon as other things are done.