When the robot turns on the spot, its reported position should not change. However, the position of the camera does change, meaning we need to do some math.

The camera is offset from the center of the robot's rotation by some x and y values. It would be simple to just subtract these from the camera's reported position, but these offsets are relative to the robot's coordinate system — not the field's coordinate system.

Solution:

1. Subtract the x and y offsets to get a new point
2. Rotate the new point around the camera by however much the robot has turned (measured by the inertial sensor)

Code Implementation:

pose.x = START_X + (cos(-radians) * CAMERA_OFFSET_X) - (sin(-radians) * CAMERA_OFFSET_Y) + converter.data.x;
pose.y = START_Y + (sin(-radians) * CAMERA_OFFSET_X) + (cos(-radians) * CAMERA_OFFSET_Y) + converter.data.y;

• radians is the current rotation of the robot
• START_X and START_Y are the starting coordinates of the robot. We need these since the camera reports its position relative to where it started, not relative to the field.
• converter.data.x and converter.data.y are the coordinates of the camera.