Kauai Labs navX-MXP User Manual page 41

Examples
Field-Oriented Drive (FRC)
of a successful robot. Omnidirectional drive systems provide motion in the Y axis (forward-
backward), X-axis (strafe), and Z axis (rotating about it's center axis). Each "degree of
freedom" is independent, meaning that the overall robot motion is comprised of a "mix" of
motion in each of the X, Y and Z axes, control of which is easily provided with a 3-degree of
freedom joystick. This resulting maneuverability is quite useful during FRC competitions to avoid
other robots, pick up and place game pieces, line up for shooting to a target, etc.
Yet the driver who remains in a fixed position is now presented a new challenge: when the
driving joystick is pushed forward, the robot does not necessarily move forward with respect to
the field – rather it moves forward with respect to the robot. This forces the driver to develop the
skill of "placing their head in the robot" and performing the angular transformation mentally.
This skill can take quite awhile to develop meaning that rookie drivers face an uphill climb
before they can be productive team contributors. Additionally, the mental energy involved in
field-to-robot rotational transformations reduces the driver's cognitive ability to focus other
game-related tactical tasks, as evidenced by drivers who are so intently focused on driving that
their response to their teammates is diminished. Moreover, when the driver does not have a
clear line of sight to the robot, the "head in the robot" becomes even more challenging.
Solving this challenge is conceptually straightforward. First, the current angle (?) of rotation
between the head of the field, and the head of the robot must be measured; secondly, the
joystick X/Y coordinates are transformed by ?, as shown in following pseudo-code:
double rcw = pJoystick->GetTwist();
double forwrd = pJoystick->GetY() * -1; /* Invert stick Y axis */
double strafe = pJoystick->GetX();
float pi = 3.1415926;
/* Adjust Joystick X/Y inputs by navX MXP yaw angle */
double gyro_degrees = ahrs->GetYaw();
float gyro_radians = gyro_degrees * pi/180;
float temp = forwrd * cos(gyro_radians) +
strafe * sin(gyro_radians);
strafe = -forwrd * sin(gyro_radians) +
An easy-to-use, highly-maneuverable drive system is at the heart
38