Universal IMU Interface

Introduction

In September 2022, REV Robotics began shipping Control Hubs with a different internal Inertial Measurement Unit (IMU). The new IMU chip is designated BHI260AP, replacing the existing Hub’s IMU chip BNO055. Both are from Bosch Sensortec. An IMU can measure many aspects of device motion; this explanatory document focuses primarily on rotation.

The Software SDK version 8.1 introduced a universal interface that supports both the BHI260AP and BNO055 IMU. This basic tutorial introduces some new features:

  • robot configuration allows selection of IMU type

  • universal classes and methods supporting both IMU types

  • three ways to specify Hub mounting orientation on the robot

Teams wanting to use the newer IMU are required to:

  • use SDK 8.1 or newer

  • update the Control Hub OS to 1.1.3 or newer.

However all teams are encouraged to begin using the universal IMU classes and methods for new Blocks and Java code. And, migrating existing code would allow you to switch easily (and perhaps urgently) to a new Control Hub during the season.

Don’t know which IMU you have? Check the Manage page under Program & Manage in any of these places:

  • connected Driver Station (DS) app

  • connected computer’s Chrome browser, at http://192.168.43.1:8080 (Control Hub) or http://192.168.49.1:8080 (RC phone)

  • REV Hardware Client (when Hub LED is green)

Each Hub’s IMU type is listed there, as of SDK 8.0.

2-hub display

Sample Control Hub and Expansion Hub display

Note

Reminder: REV Expansion Hubs purchased after December 2021 have no internal IMU.

Do you have existing OpModes using the original IMU? Your code can run unchanged, using Hubs with the BNO055. The new SDK 8.1 fully supports legacy Blocks and Java code using classes and methods for the BNO055 IMU.

The SDK 8.1 README provides more technical background:

Unlike the old BNO055IMU interface, which only worked correctly when the
REV Hub was mounted flat on your robot, the IMU interface allows you to
specify the orientation of the REV Hub on your robot. It will account for
this, and give you your orientation in a Robot Coordinate System, instead of
a special coordinate system for the REV Hub. As a result, your pitch and yaw
will be 0 when your robot is level, instead of when the REV Hub is level,
which will result in much more reliable orientation angle values for most
mounting orientations.
If you have calibrated your BNO055, you can provide that calibration data to
the new IMU interface by passing a BNO055IMUNew.Parameters instance
to IMU.initialize().
Because of the new robot-centric coordinate system, the pitch and roll
angles returned by the IMU interface will be different from the ones
returned by the BNO055IMU interface. When you are migrating your code,
pay careful attention to the documentation.

Potential Usage

FIRST Tech Challenge robots drive mostly on a flat playing field, typically using the IMU to monitor or control Heading (Yaw or Z-angle).

Heading is preserved between OpMode runs, unless the robot or Robot Controller (RC) app are restarted. This can be useful between Autonomous and TeleOp. Heading can be reset during an OpMode, as discussed below.

Heading can drift slowly over time. An absolute reference is not available from gravity or from a magnetometer, which can be affected by nearby motors. This ‘Yaw drift’ is discussed below.

The IMU can help with more than Heading! Some FIRST Tech Challenge games have placed robots on tilted surfaces:

previous games 1
previous games 2

Sample images from previous games utilizing tilted surfaces

(Block Party!, FIRST RES-Q, Relic Recovery, Face Off!, Get Over It!)

Such fields, and special circumstances in any FIRST Tech Challenge game, may cause teams to seek IMU readings for Pitch and Roll angles.

Examples might include:

  • robot’s left wheels are raised, on an obstacle

  • robot is tilted forward on its front 4 wheels (of 6-wheel West Coast Drive)

  • robot has tipped over (!)

  • robot’s secondary Expansion Hub (with IMU) is mounted on a tilting mechanism

The Software SDK can also provide values for angular velocity, which is the rate of change (degrees per second) for Roll, Pitch or Yaw.

Let’s get started!

Configure IMU

Robot configuration of the IMU is automatic, and shouldn’t need changes. But here’s how to confirm or rename your configured IMU.

In a connected DS app, touch the 3-dots icon at top right, then touch Configure Robot. For any new or existing Configuration, touch Control Hub Portal, then select the Hub with the IMU you want to use. Typically this will be the Control Hub, whether old or new.

IMU Robot Configuration

REV IMU Robot Configuration Validation

  • Yellow: The internal IMU is (always) connected at I2C Bus 0, Port 0. If you want another I2C device also on Bus 0, plug it into the Hub and use the Add button.

  • Green: The default IMU type shown will reflect the actual unit in this Hub; fix this only if it was incorrectly modified. Your IMU OpModes require a correct choice here.

  • Purple: The default device name is “imu”, used by all Sample OpModes for Blocks and Java. You may enter a custom name here, but you must then update all your OpModes that reference the IMU.

When done, save and activate this configuration.

If a Blocks OpMode is open at the computer’s programming screen, close and re-open that OpMode to capture this updated configuration. Blocks are provided only for devices in the configuration that’s active upon opening an OpMode.

Axes Definition

Robot orientation is defined using the Robot Coordinate System, with 3 axes that are orthogonal (at 90 degrees to each other), with origin inside the robot.

You must decide which face or direction is “forward” on your robot (which could be round!).

Tip

Placing a tape label “FRONT” at the team-agreed front face or front edge of the robot can avoid confusion later – really!

  • Heading, or Yaw, is the measure of rotation about the Z axis, which points up toward the ceiling.

  • Pitch is the measure of rotation about the X axis, which points out the right side of the robot.

  • Roll is the measure about the Y axis, which points out the front of the robot.

These are Robot axes, different than (and not aligned with) the Hub axes used by the legacy BNO055IMU driver.

Rotation follows the traditional right-hand rule: with the thumb pointing along the positive axis, the fingers curl in the direction of positive rotation.

Hint

Fun fact: the IMU is located approximately under the word “PROUD”, near the lower right corner of the Hub.

This tutorial will not discuss the FIRST Tech Challenge Field Coordinate System. Your OpModes might relate robot orientation to the overall field or ‘global coordinates’ for navigation, but that’s beyond the focus here on using the IMU.

Physical Hub Mounting

Under SDK 8.1, you can specify the physical orientation of the Hub on the robot. This allows you to receive IMU angle values expressed in robot axes, useful for understanding and managing the robot’s movement.

Before jumping into programming, let’s discuss your options for physically mounting the Hub on the robot. In general, the Hub’s mounting can be considered Orthogonal or Non-Orthogonal.

Orthogonal Mounting

Imagine a cube anywhere on your robot, parallel to the floor, with one flat side facing exactly towards the designated “front” of your robot. Place your Hub on one of these cube faces, with the Hub’s straight edges parallel to the cube.

If that describes the orientation of your Hub, use the Orthogonal method of specifying its orientation. See the IMU Programming section below.

Here are some common examples:

Orthogonal #1

Logo UP, USB Forward

Orthogonal #2

Logo LEFT, USB UP

Orthogonal #3

Logo RIGHT, USB UP

Orthogonal #4

Logo FORWARD, USB UP

Orthogonal #5

Logo BACKWARD, USB UP

Orthogonal #6

Logo DOWN, USB FORWARD

Orthogonal #7

Logo FORWARD, USB LEFT

Orthogonal #8

Logo FORWARD, USB RIGHT

Orthogonal #9

Logo UP, USB BACKWARD

With six cube faces, and four 90-degree positions on each face, there are 24 possible Orthogonal orientations.

Non-Orthogonal Mounting

Here are some scenarios, ranging from simple to complex:

  • Imagine the same front-aligned cube, with your Hub on any face. The Hub’s edges are not parallel to the cube. Namely, the Hub is rotated only in-plane (clockwise or counter-clockwise, looking at the REV logo).

  • The Hub is mounted/tilted at some oblique angle from a face on the imaginary cube. At that single tilted angle, the Hub is not rotated in-plane (clockwise or counter-clockwise, looking at the logo).

  • The Hub is tilted at multiple angles, with or without in-plane rotation.

For any Non-Orthogonal scenarios, SDK 8.1 provides two ways to describe the Hub’s orientation. See below for the Angles method and the Quaternion method.

IMU Programming

SDK 8.1 offers new classes and methods that apply universally to both types of IMU. Once configured, the IMU type will not affect your programming. The programming steps include:

  • set the IMU parameters, or use defaults

  • initialize the IMU

  • read values from the IMU, use as needed to control the robot

  • optional: reset Heading one or more times

The following sections cover these topics in order.

Parameters

There are three ways to describe the Hub’s orientation, using IMU parameters. One is for Orthogonal mounting, and two are for Non-Orthogonal mounting. Choose the simplest method that applies to your robot.

As an example, in the FIRST Tech Challenge Blocks menu under Sensors and IMU, you can find these three methods for specifying parameters:

Sample Blocks screenshot demonstrating parameter methods

Sample Blocks screenshot, demonstrating the three parameter methods

Parameters for Method 1, Orthogonal

Method 1 consists of supplying a simple Orthogonal configuration. This requires you to determine the direction that the REV logo is facing. To do this, consider the Hub is mounted on an imaginary cube aligned to the “front” of the robot. Specify the Hub’s mounting face: “Forward” means robot forward (front face), “Left” means robot left, etc.

Next, choose how the Hub is rotated on that face. Use the USB ports at the “top” of the Hub to determine this direction; assume you are at the rear of the robot, looking “forward”.

Note

Certain combinations are physically impossible. For example, if the REV logo is facing UP, the USB ports cannot also be facing UP. The OpMode will reject such combinations during IMU initialization.

It’s optional to save the parameters to a new variable called, for example, “myIMUparameters”. That variable can be used in the next step (IMU initialization).

specifying Logo Facing Direction

Specifying Logo Facing Direction

specifying USB Facing Direction

Specifying USB Facing Direction

Setting parameters to variable

Setting parameters to a Variable

IMU.Parameters myIMUparameters;

myIMUparameters = new IMU.Parameters(
     new RevHubOrientationOnRobot(
          RevHubOrientationOnRobot.LogoFacingDirection.UP,
          RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
     )
);

Hub Axes for Setting Parameters

Only for the next two Parameters sections (Angles and Quaternion), we must temporarily use Hub axes instead of Robot axes. Hub axes are also at 90 degrees to each other, with origin inside the Hub.

The assumed initial Hub position is REV logo facing UP (Robot +Z), with USB ports FORWARD (Robot +Y). For the Angles and Quaternion methods, all rotations start here.

Again, “forward” is based on your team’s agreed definition.

In this starting orientation, the Hub axes are aligned with the Robot Coordinate System:

  • Heading, or Yaw, is the measure of rotation about the Z axis, which points upwards through the Hub’s front plate or logo.

  • Pitch is the measure of rotation about the X axis, which points toward the right-side I2C sensor ports.

  • Roll is the measure about the Y axis, which points toward the top-edge USB port(s).

Hub rotations also follow the right-hand rule.

The legacy BNO055IMU driver uses different Hub axes: its X axis pointed to the USB port, and Y axis pointed to the left-side motor ports. The new SDK 8.1 universal IMU driver uses the above Hub axes for BNO055 and BHI260AP.

Parameters for Method 2, Angles

If your Hub is not mounted Orthogonally, you can specify the Hub’s rotation about one or more Hub axes X, Y, Z. These are expressed in degrees, and the order in which the rotations are applied (it matters!).

The Blocks IMU palette contains a Block with default parameters for the Angles method of describing the Hub’s orientation on the robot. Let’s review this Blocks palette function now, as a good example. The Java API closely resembles the Blocks method.

Sample Block demonstrating angles method

Sample Block demonstrating angles method

The second listed default is ZYX, meaning you will provide the Hub’s rotations in that order. Thus the “first angle” is the Z axis, the “second angle” is the Y axis, and the “third angle” is the X axis.

So the Hub will be rotated as follows: +90 degrees about Z, no rotation about Y, then -45 degrees about X (in its new direction).

For the Angles method, the assumed initial Hub position is REV Logo facing UP, with USB ports facing FORWARD. Additional rotations begin at this orientation.

  1. From logo-up/USB-forward, this example starts with a “first angle” rotation of +90 degrees about the Z axis. Namely, the Hub rotates counter-clockwise (CCW), ending with the USB ports pointing to the robot’s left side. Note the X and Y axes have also rotated CCW, since they are INTRINSIC (described below).

  2. The “second angle” rotation is 0 degrees, no action.

  3. The “third angle” rotation is -45 degrees about the Hub’s X axis, which now points in the robot’s forward direction (after the first-angle Z rotation). So, the top edge of the Hub tilts downward, causing the USB ports to angle downward at 45 degrees, at the robot’s left side.

Here’s the full sequence:

Angles Rotation Step #1

Starting Position

Angles Rotation Step #2

First Angle (Z axis +90)

Angles Rotation Step #3

Third Angle (X axis -45)

The remaining default parameters don’t need attention or editing. The third listed default is simply DEGREES, easy to work with. The first listed default is INTRINSIC axes reference, which means that the Hub axes move with each rotation of the Hub. (The other choice, rarely used, is EXTRINSIC for global axes that don’t move with each Hub rotation.)

As with Orthogonal, it’s optional to save the parameters to a new variable called, for example, “myIMUparameters”. That variable can be used in the next step (IMU initialization).

setting angles in Blocks

Setting Angles in Blocks

IMU.Parameters myIMUparameters;

myIMUparameters = new IMU.Parameters(
     new RevHubOrientationOnRobot(
          new Orientation(
               AxesReference.INTRINSIC,
               AxesOrder.ZYX,
               AngleUnit.DEGREES,
               90,
               0,
               -45,
               0  // acquisitionTime, not used
          )
     )
);

Parameters for Method 3, Quaternion

As an alternative to the Angles method, the Hub’s non-orthogonal orientation can be described using a Quaternion, an advanced math technique for describing any combination of tilting and rotating.

The following default Quaternion (w=1, x=0, y=0, z=0) describes a Hub in the assumed starting position: Logo facing UP, and USB ports FORWARD. Namely, no rotations.

default quaternion

Default Quaternion (no rotation)

IMU.Parameters myIMUparameters;

// Default Quaternion
myIMUparameters = new IMU.Parameters(
     new RevHubOrientationOnRobot(
          new Quaternion(
               1.0f, // w
               0.0f, // x
               0.0f, // y
               0.0f, // z
               0     // acquisitionTime
          )
     )
);

// Or, consider a single rotation of +30 degrees
// about the X axis. Namely, the Hub’s USB ports
// tilt 30 degrees upwards from the default starting
// position.
myIMUparameters = new IMU.Parameters(
     new RevHubOrientationOnRobot(
          new Quaternion(
               0.9659258f, // w
               0.258819f,  // x
               0.0f,       // y
               0.0f,       // z
               0           // acquisitionTime
          )
     )
);

This basic tutorial does not cover the math behind Quaternions, an advanced substitute for Euler Angles described above. The SDK 8.1 IMU interface supports the use of Quaternions, for teams and third party libraries familiar with them.

Initialize IMU

This prepares the IMU for operation, using the parameters you defined.

In Blocks, use the first Block shown in the IMU palette, called imu.initialize. Most teams do this during the INIT phase of their OpMode, before waitForStart().

The IMU should be motionless during its initialization process. The OpMode will continue when initialization is complete.

Note

Fun fact: Under the legacy BNO055IMU interface, intialization takes about 900 milliseconds. Under the new universal IMU interface, the BNO055 takes about 100 milliseconds, while the BHI260AP takes about 50 milliseconds.

For any of the three methods (Orthogonal, Angles, Quaternion), initialize with the IMU parameters from the new Block, or from your optional Variable.

Two methods for Initializing the IMU:

Initialize IMU directly

Initializing the IMU directly

Initialize IMU using Parameters

Initializing the IMU using Parameters

// Two methods for Initializing the IMU:

// Initialize IMU directly
imu.initialize(
     new IMU.Parameters(
          new RevHubOrientationOnRobot(
               RevHubOrientationOnRobot.LogoFacingDirection.UP,
               RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
          )
     )
);

// Initialize IMU using Parameters
imu.initialize(myIMUparameters);

Read IMU Angles - Basic

Now you can read the IMU values for robot orientation, expressed as Heading (Yaw or Z-angle), Pitch (X-angle) and Roll (Y-angle). You have no concern now about the Hub’s orientation or mounting – that has been defined with parameters, and the SDK is ready to provide actual data about the robot, using the robot’s axes.

Note

Reminder: Robot Z points upwards to the ceiling. Robot Y points forward – whatever you decide is “forward” on your robot (which could be round!). Robot X points to the right side of the robot. Robot rotations follow the right-hand rule.

For all axes, IMU angles are provided in the range of -180 to +180 degrees (or from -Pi to +Pi radians). If you are working with values that might cross the +/- 180-degree transition, handle this with your programming. That’s beyond the scope of this IMU tutorial.

Here’s an example of reading IMU Angles:

In Blocks, create a new Variable to receive data from this green Block in the IMU palette:

Get YPR Angles

Get Yaw-Pitch-Roll Angles

From the YawPitchRollAngles palette under Utilities, use the green Blocks to read each angle from the Variable you just created.

Extract Angles

Extract Angles

These Blocks are used here in a Repeat Loop, to display the angles on the Driver Station:

Displaying YPR using Telemetry

Displaying Yaw-Pitch-Roll using Telemetry

These Blocks are shown in the Sample OpMode called SensorIMU.

// Create an object to receive the IMU angles
YawPitchRollAngles robotOrientation;
robotOrientation = imu.getRobotYawPitchRollAngles();

// Now use these simple methods to extract each angle
// (Java type double) from the object you just created:
double Yaw   = robotOrientation.getYaw(AngleUnit.DEGREES);
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
double Roll  = robotOrientation.getRoll(AngleUnit.DEGREES);

Note that the robot’s orientation is described here intrinsically; the axes move with each rotation. Here’s an example from the Javadocs:

As an example, if the yaw is 30 degrees, the pitch is 40 degrees, and
the roll is 10 degrees, that means that you would reach the described
orientation by first rotating a robot 30 degrees counter-clockwise from
the starting point, with all wheels continuing to touch the ground
(rotation around the Z axis). Then, you make your robot point 40 degrees
upward (rotate it 40 degrees around the X axis). Because the X axis
moved with the robot, the pitch is not affected by the yaw value. Then
from that position, the robot is tilted 10 degrees to the right, around
the newly positioned Y axis, to produce the actual position of the
robot.

Again, the IMU output results are given in the Robot Coordinate System, or Robot axes. Only for a non-Orthogonal orientation, Hub axes were used temporarily for input parameters, describing the Hub’s rotation to achieve its mounted orientation.

Read IMU Angles - Flexible

As an alternative to the YawPitchRollAngles class, the SDK also provides a more flexible Orientation class. This allows you to specify a custom order of axis rotations, and a choice of intrinsic or extrinsic axes.

Again, IMU angles are provided in the range of -180 to +180 degrees (or from -Pi to +Pi radians).

Here is an example use of these functions:

As before, first create an object (Blocks Variable) containing the array of orientation values (from the Blocks Sensors / IMU palette):

Get Robot Orientation

Get Robot Orientation

Notice the axes order of XYZ, different than the ZXY order used by the YawPitchRollAngles class.

Then extract the specific axis rotations you want, from the Blocks Utilities / Orientation palette:

Extract Orientation Angles

Extract Orientation Angles

// Create Orientation variable
Orientation myRobotOrientation;

// Get Robot Orientation
myRobotOrientation = imu.getRobotOrientation(
     AxesReference.INTRINSIC,
     AxesOrder.XYZ,
     AngleUnit.DEGREES
);

// Then read or display the desired values (Java type float):
float X_axis = myRobotOrientation.firstAngle;
float Y_axis = myRobotOrientation.secondAngle;
float Z_axis = myRobotOrientation.thirdAngle;

Note

Pay close attention to the selection of axes order, which greatly affects the IMU results. If you care mostly about Heading (Yaw), choose an axes order that starts with Z.

Read Angular Velocity

The SDK also provides values for angular velocity, the rate of change (degrees or radians per second) for Roll, Pitch or Yaw.

Here is an example for reading Angular Velocity:

As before, first create an object (Blocks Variable) containing the array of angular velocity values (from the Blocks Sensors / IMU palette):

Get Robot Angular Velocity

Get Robot Angular Velocity

Then extract the specific axis rotations you want, from the Blocks Utilities / AngularVelocity palette:

Extract Rotation Rates

Extract Rotation Rates

These Blocks are shown in the Sample OpMode called SensorIMU.

// Create angular velocity array variable
AngularVelocity myRobotAngularVelocity;

// Read Angular Velocities
myRobotAngularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);

// Then read or display these values (Java type float)
// from the object you just created:
float zRotationRate = myRobotAngularVelocity.zRotationRate;
float xRotationRate = myRobotAngularVelocity.xRotationRate;
float yRotationRate = myRobotAngularVelocity.yRotationRate;

These are also shown in each of the Java Sample OpModes listed in a section below.

Reset Heading

It can be useful to reset the Heading (or Yaw or Z-angle) to zero, at one or more places in your OpMode.

Here is an example for resetting the Yaw axis:

In Blocks, this optional command is simple:

Reset Yaw

Reset Yaw

// Reset Yaw
imu.resetYaw();

It’s safest to reset Yaw only when the robot has not significantly deviated from a flat/horizontal orientation.

This command assumes the Hub’s actual orientation was correctly described with Orthogonal, Angles or Quaternion parameters.

In other words, a non-Orthogonal Hub moved away from its parameter-defined orientation, may not give reliable results for Heading/Yaw or resetYaw(), even after the robot has returned to its original defined orientation.

An exception, or loophole, is that “reset” Heading/Yaw values might still be valid if the Hub is actually mounted in an incorrectly described Orthogonal orientation, and the robot remains level. This may benefit a rookie team that overlooked the IMU Parameters or moved the Hub to a different Orthogonal position, still relying only on Heading. This resetYaw() exception does not apply to angular velocity for Yaw (Z-axis).

Here’s the official Javadocs description for resetYaw():

Resets the robot’s yaw angle to 0. After calling this method, the reported
orientation will be relative to the robot’s position when this method is
called, as if the robot was perfectly level right now. That is to say, the
pitch and yaw will be ignored when this method is called.

The Javadocs’ statement ‘resets to 0’ should be read in the context of the previous discussion. In certain off-axis Hub orientations, a reset Yaw value might not actually display as zero.

If resetYaw() does not meet your needs, other code-based choices (possibly less effective) include:

  • ‘Save & Subtract’ to establish the current Heading as a new “zero” baseline for further navigation

  • use the original Heading for the entire match, using only absolute (global) targets

Important

For all choices, be aware of “gyro drift”. Most electronic IMUs give slowly shifting Z-angle results over time, for various reasons. Although the Pitch and Roll axes can use gravity’s direction to correct for drift, Yaw (Heading or Z-angle) cannot.

Sample OpModes

SDK 8.1 and newer contains Sample OpModes demonstrating the above.

In Blocks, a simple example is called SensorIMU.

Blocks IMU Sample

Blocks IMU Sample

Here’s an image and the Blocks file of this Sample OpMode.

In Java, three Sample OpModes demonstrate the new universal IMU interface:

ConceptExploringIMUOrientation.java

Provides a tool to experiment with setting your Hub orientation on the robot

ConceptExploringIMUOrientation.java

/*
Copyright (c) 2022 REV Robotics, FIRST
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted (subject to the limitations in the disclaimer below) provided that
the following conditions are met:
Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.
Neither the name of REV Robotics nor the names of its contributors may be used to
endorse or promote products derived from this software without specific prior
written permission.
NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;

import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.IMU;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;

/**
 * This file demonstrates the impact of setting the IMU orientation correctly or incorrectly. This
 * code assumes there is an IMU configured with the name "imu".
 * <p>
 * Note: This OpMode is more of a tool than a code sample. The User Interface portion of this code
 *       goes beyond simply showing how to interface to the IMU.<br>
 *       For a minimal example of interfacing to an IMU, please see the SensorIMUOrthogonal or SensorIMUNonOrthogonal sample OpModes.
 * <p>
 * This sample enables you to re-specify the Hub Mounting orientation dynamically by using gamepad controls.
 * While doing so, the sample will display how Pitch, Roll and Yaw angles change as the hub is moved.
 * <p>
 * The gamepad controls let you change the two parameters that specify how the Control/Expansion Hub is mounted. <br>
 * The first parameter specifies which direction the printed logo on the Hub is pointing. <br>
 * The second parameter specifies which direction the USB connector on the Hub is pointing. <br>
 * All directions are relative to the robot, and left/right is as viewed from behind the robot.
 * <p>
 * How will you know if you have chosen the correct Orientation? With the correct orientation
 * parameters selected, pitch/roll/yaw should act as follows:
 * <p>
 *   Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
 *   Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
 *   Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
 * <p>
 * The Yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
 * <p>
 * The rotational velocities should follow the change in corresponding axes.
 */

@TeleOp(name="Concept: IMU Orientation", group="Concept")
@Disabled
public class ConceptExploringIMUOrientation extends LinearOpMode {
    static RevHubOrientationOnRobot.LogoFacingDirection[] logoFacingDirections
            = RevHubOrientationOnRobot.LogoFacingDirection.values();
    static RevHubOrientationOnRobot.UsbFacingDirection[] usbFacingDirections
            = RevHubOrientationOnRobot.UsbFacingDirection.values();
    static int LAST_DIRECTION = logoFacingDirections.length - 1;
    static float TRIGGER_THRESHOLD = 0.2f;

    IMU imu;
    int logoFacingDirectionPosition;
    int usbFacingDirectionPosition;
    boolean orientationIsValid = true;

    @Override public void runOpMode() throws InterruptedException {
        imu = hardwareMap.get(IMU.class, "imu");
        logoFacingDirectionPosition = 0; // Up
        usbFacingDirectionPosition = 2; // Forward

        updateOrientation();

        boolean justChangedLogoDirection = false;
        boolean justChangedUsbDirection = false;

        // Loop until stop requested
        while (!isStopRequested()) {

            // Check to see if Yaw reset is requested (Y button)
            if (gamepad1.y) {
                telemetry.addData("Yaw", "Resetting\n");
                imu.resetYaw();
            } else {
                telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset.\n");
            }

            // Check to see if new Logo Direction is requested
            if (gamepad1.left_bumper || gamepad1.right_bumper) {
                if (!justChangedLogoDirection) {
                    justChangedLogoDirection = true;
                    if (gamepad1.left_bumper) {
                        logoFacingDirectionPosition--;
                        if (logoFacingDirectionPosition < 0) {
                            logoFacingDirectionPosition = LAST_DIRECTION;
                        }
                    } else {
                        logoFacingDirectionPosition++;
                        if (logoFacingDirectionPosition > LAST_DIRECTION) {
                            logoFacingDirectionPosition = 0;
                        }
                    }
                    updateOrientation();
                }
            } else {
                justChangedLogoDirection = false;
            }

            // Check to see if new USB Direction is requested
            if (gamepad1.left_trigger > TRIGGER_THRESHOLD || gamepad1.right_trigger > TRIGGER_THRESHOLD) {
                if (!justChangedUsbDirection) {
                    justChangedUsbDirection = true;
                    if (gamepad1.left_trigger > TRIGGER_THRESHOLD) {
                        usbFacingDirectionPosition--;
                        if (usbFacingDirectionPosition < 0) {
                            usbFacingDirectionPosition = LAST_DIRECTION;
                        }
                    } else {
                        usbFacingDirectionPosition++;
                        if (usbFacingDirectionPosition > LAST_DIRECTION) {
                            usbFacingDirectionPosition = 0;
                        }
                    }
                    updateOrientation();
                }
            } else {
                justChangedUsbDirection = false;
            }

            // Display User instructions and IMU data
            telemetry.addData("logo Direction (set with bumpers)", logoFacingDirections[logoFacingDirectionPosition]);
            telemetry.addData("usb Direction (set with triggers)", usbFacingDirections[usbFacingDirectionPosition] + "\n");

            if (orientationIsValid) {
                YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
                AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);

                telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
                telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
                telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
                telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
                telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
                telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
            } else {
                telemetry.addData("Error", "Selected orientation on robot is invalid");
            }

            telemetry.update();
        }
    }

    // apply any requested orientation changes.
    void updateOrientation() {
        RevHubOrientationOnRobot.LogoFacingDirection logo = logoFacingDirections[logoFacingDirectionPosition];
        RevHubOrientationOnRobot.UsbFacingDirection usb = usbFacingDirections[usbFacingDirectionPosition];
        try {
            RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logo, usb);
            imu.initialize(new IMU.Parameters(orientationOnRobot));
            orientationIsValid = true;
        } catch (IllegalArgumentException e) {
            orientationIsValid = false;
        }
    }
}
SensorIMUOrthogonal.java

Shows how to define your Hub orientation on the robot, for simple orthogonal (90 degree) mounting

SensorIMUOrthogonal.java

/* Copyright (c) 2022 FIRST. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted (subject to the limitations in the disclaimer below) provided that
 * the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this list
 * of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of FIRST nor the names of its contributors may be used to endorse or
 * promote products derived from this software without specific prior written permission.
 *
 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
 * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

package org.firstinspires.ftc.robotcontroller.external.samples;

import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.IMU;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;

/**
 * {@link SensorIMUOrthogonal} shows how to use the new universal {@link IMU} interface. This
 * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
 * on the robot with the name "imu".
 * <p>
 * The sample will display the current Yaw, Pitch and Roll of the robot.<br>
 * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
 * <p>
 *   Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
 *   Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
 *   Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
 * <p>
 * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
 * <p>
 * This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
 * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
 * <p>
 * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
 *       the alternative SensorImuNonOrthogonal sample in this folder.
 * <p>
 * This "Orthogonal" requirement means that:
 * <p>
 * 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
 *    FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
 * <p>
 * 2) The USB ports can only be pointing in one of the same six directions:<br>
 *    FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
 * <p>
 * So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
 *    logoFacingDirection<br>
 *    usbFacingDirection
 * <p>
 * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
 * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
 * <p>
 * Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
 * to use those parameters.
 */
@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
@Disabled   // Comment this out to add to the OpMode list
public class SensorIMUOrthogonal extends LinearOpMode
{
    // The IMU sensor object
    IMU imu;

    //----------------------------------------------------------------------------------------------
    // Main logic
    //----------------------------------------------------------------------------------------------

    @Override public void runOpMode() throws InterruptedException {

        // Retrieve and initialize the IMU.
        // This sample expects the IMU to be in a REV Hub and named "imu".
        imu = hardwareMap.get(IMU.class, "imu");

        /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
         *
         * Two input parameters are required to fully specify the Orientation.
         * The first parameter specifies the direction the printed logo on the Hub is pointing.
         * The second parameter specifies the direction the USB connector on the Hub is pointing.
         * All directions are relative to the robot, and left/right is as-viewed from behind the robot.
         */

        /* The next two lines define Hub orientation.
         * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
         *
         * To Do:  EDIT these two lines to match YOUR mounting configuration.
         */
        RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
        RevHubOrientationOnRobot.UsbFacingDirection  usbDirection  = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;

        RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);

        // Now initialize the IMU with this mounting orientation
        // Note: if you choose two conflicting directions, this initialization will cause a code exception.
        imu.initialize(new IMU.Parameters(orientationOnRobot));

        // Loop and update the dashboard
        while (!isStopRequested()) {

            telemetry.addData("Hub orientation", "Logo=%s   USB=%s\n ", logoDirection, usbDirection);

            // Check to see if heading reset is requested
            if (gamepad1.y) {
                telemetry.addData("Yaw", "Resetting\n");
                imu.resetYaw();
            } else {
                telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
            }

            // Retrieve Rotational Angles and Velocities
            YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
            AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);

            telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
            telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
            telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
            telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
            telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
            telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
            telemetry.update();
        }
    }
}
SensorIMUNonOrthogonal.java

Shows how to define (with the Angles method) your Hub orientation on the robot for a non-orthogonal orientation

SensorIMUNonOrthogonal.java

/* Copyright (c) 2022 FIRST. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted (subject to the limitations in the disclaimer below) provided that
 * the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this list
 * of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of FIRST nor the names of its contributors may be used to endorse or
 * promote products derived from this software without specific prior written permission.
 *
 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
 * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

package org.firstinspires.ftc.teamcode;

import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.xyzOrientation;

import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.IMU;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;

/**
 * {@link SensorIMUNonOrthogonal} shows how to use the new universal {@link IMU} interface. This
 * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
 * on the robot with the name "imu".
 * <p>
 * The sample will display the current Yaw, Pitch and Roll of the robot.<br>
 * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
 * <p>
 *   Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
 *   Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
 *   Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
 * <p>
 * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
 * <p>
 * This specific sample DOES NOT assume that the Hub is mounted on one of the three orthogonal
 * planes (X/Y, X/Z or Y/Z) OR that the Hub has only been rotated in a range of 90 degree increments.
 * <p>
 * Note: if your Hub is mounted Orthogonally (on a orthogonal surface, angled at some multiple of
 * 90 Degrees) then you should use the simpler SensorImuOrthogonal sample in this folder.
 * <p>
 * But... If your Hub is mounted Non-Orthogonally, you must specify one or more rotational angles
 * that transform a "Default" Hub orientation into your desired orientation.  That is what is
 * illustrated here.
 * <p>
 * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
 * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
 * <p>
 * Finally, edit this OpMode to use at least one angle around an axis to orient your Hub.
 */
@TeleOp(name = "Sensor: IMU Non-Orthogonal", group = "Sensor")
@Disabled     // Comment this out to add to the OpMode list
public class W_nonortho extends LinearOpMode
{
    // The IMU sensor object
    IMU imu;

    //----------------------------------------------------------------------------------------------
    // Main logic
    //----------------------------------------------------------------------------------------------

    @Override public void runOpMode() throws InterruptedException {

        // Retrieve and initialize the IMU.
        // This sample expects the IMU to be in a REV Hub and named "imu".
        imu = hardwareMap.get(IMU.class, "imu");

        /* Define how the hub is mounted to the robot to get the correct Yaw, Pitch and Roll values.
         *
         * You can apply up to three axis rotations to orient your Hub according to how it's mounted on the robot.
         *
         * The starting point for these rotations is the "Default" Hub orientation, which is:
         * 1) Hub laying flat on a horizontal surface, with the Printed Logo facing UP
         * 2) Rotated such that the USB ports are facing forward on the robot.
         *
         * The order that the rotations are performed matters, so this sample shows doing them in the order X, Y, then Z.
         * For specifying non-orthogonal hub mounting orientations, we must temporarily use axes
         * defined relative to the Hub itself, instead of the usual Robot Coordinate System axes
         * used for the results the IMU gives us. In the starting orientation, the Hub axes are
         * aligned with the Robot Coordinate System:
         *
         * X Axis:  Starting at Center of Hub, pointing out towards I2C connectors
         * Y Axis:  Starting at Center of Hub, pointing out towards USB connectors
         * Z Axis:  Starting at Center of Hub, pointing Up through LOGO
         *
         * Positive rotation is defined by right-hand rule with thumb pointing in +ve direction on axis.
         *
         * Some examples.
         *
         * ----------------------------------------------------------------------------------------------------------------------------------
         * Example A) Assume that the hub is mounted on a sloped plate at the back of the robot, with the USB ports coming out the top of the hub.
         *  The plate is tilted UP 60 degrees from horizontal.
         *
         *  To get the "Default" hub into this configuration you would just need a single rotation.
         *  1) Rotate the Hub +60 degrees around the X axis to tilt up the front edge.
         *  2) No rotation around the Y or Z axes.
         *
         *  So the X,Y,Z rotations would be 60,0,0
         *
         * ----------------------------------------------------------------------------------------------------------------------------------
         * Example B) Assume that the hub is laying flat on the chassis, but it has been twisted 30 degrees towards the right front wheel to make
         *  the USB cable accessible.
         *
         *  To get the "Default" hub into this configuration you would just need a single rotation, but around a different axis.
         *  1) No rotation around the X or Y axes.
         *  1) Rotate the Hub -30 degrees (Clockwise) around the Z axis, since a positive angle would be Counter Clockwise.
         *
         *  So the X,Y,Z rotations would be 0,0,-30
         *
         * ----------------------------------------------------------------------------------------------------------------------------------
         *  Example C) Assume that the hub is mounted on a vertical plate on the right side of the robot, with the Logo facing out, and the
         *  Hub rotated so that the USB ports are facing down 30 degrees towards the back wheels of the robot.
         *
         *  To get the "Default" hub into this configuration will require several rotations.
         *  1) Rotate the hub +90 degrees around the X axis to get it standing upright with the logo pointing backwards on the robot
         *  2) Next, rotate the hub +90 around the Y axis to get it facing to the right.
         *  3) Finally rotate the hub +120 degrees around the Z axis to take the USB ports from vertical to sloping down 30 degrees and
         *     facing towards the back of the robot.
         *
         *  So the X,Y,Z rotations would be 90,90,120
         */

        // The next three lines define the desired axis rotations.
        // To Do: EDIT these values to match YOUR mounting configuration.
        double xRotation = 0;  // enter the desired X rotation angle here.
        double yRotation = 0;  // enter the desired Y rotation angle here.
        double zRotation = 0;  // enter the desired Z rotation angle here.

        Orientation hubRotation = xyzOrientation(xRotation, yRotation, zRotation);

        // Now initialize the IMU with this mounting orientation
        RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(hubRotation);
        imu.initialize(new IMU.Parameters(orientationOnRobot));

        // Loop and update the dashboard
        while (!isStopRequested()) {
            telemetry.addData("Hub orientation", "X=%.1f,  Y=%.1f,  Z=%.1f \n", xRotation, yRotation, zRotation);

            // Check to see if heading reset is requested
            if (gamepad1.y) {
                telemetry.addData("Yaw", "Resetting\n");
                imu.resetYaw();
            } else {
                telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
            }

            // Retrieve Rotational Angles and Velocities
            YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
            AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);

            telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
            telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
            telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
            telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
            telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
            telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
            telemetry.update();
        }
    }
}

These three Java samples include extensive comments describing the IMU interface, consistent with this tutorial. In particular, SensorIMUNonOrthogonal.java describes three helpful examples.

SDK Resources

Advanced programmers are invited to browse the Javadocs documentation (API), particularly in:

  • com.qualcomm.robotcore.hardware

  • org.firstinspires.ftc.robotcore.external.navigation

The new universal IMU classes for SDK 8.1 are:

  • IMU

  • ImuOrientationOnRobot

  • YawPitchRollAngles

  • RevHubOrientationOnRobot

The Javadocs describe other IMU methods and variables not covered in this basic tutorial.

Summary

The SDK 8.1 provides a universal interface that supports both the BHI260AP and BNO055 IMU. This basic tutorial introduced some new features:

  • robot configuration allows selection of IMU type

  • three ways to specify Hub mounting orientation on the robot

  • new Blocks and Java methods to read data from both IMU types

Teams using the new Control Hub IMU must use at least SDK 8.1 AND must update to at least Control Hub OS 1.1.3.

However all teams are encouraged to begin using the universal IMU classes and methods for new Blocks and Java code, and consider migrating existing code.

Questions, comments and corrections to westsiderobotics@verizon.net