Kauai Labs navX-MXP User Manual page 49

Examples
Automatic Balancing (FRC)
Teleop.vi
The Teleop.vi is modified to feed the current navX-MXP "Yaw" angle reading to the Holonomic
Drive VI, which rotates the joystick X/Y coordinates by the gyro angle (and thus implements
FieldCentric drive control). Additionally, if a driver joystick button is pressed, the navX-MXP
"Yaw" angle is reset to zero. Finally, the navX-MXP "Pitch" (X-axis) and "Roll" (Y-axis) angles
are continuously compared to a "out of balance" threshold, at which point the corresponding
axis motor output value is derived from the inverse of the sin of that angle, until the time when
that same angle falls below the "in balance" threshold.
46