Updated position/orientation

Used output from homogeneous transform matrix to determine position and
orientation of the robot. position is measured relative to the center of
the rink, and orientation is measured in radians (counter-clockwise =
positive) relative to the rink coordinate frame.