SensorIMUOrthogonal_Blocks Sample Program

This is a tutorial on creating first FIRST Tech Challenge (FTC) autonomous programs for the CENTERSTAGE game.

Visit the FTC docs page on Creating Op Modes in blocks if you need help getting started and to create your first driver controlled program.

Prerequisites/Assumptions

This tutorial assumes you’ve got some familiarity with Blocks. Ideally you’ve completed a Blocks tutorial. This tutorial doesn’t explain how to program in blocks.

What is an IMU?

An Inertial Measurement Unit (IMU) is an electronic device that measures and reports a body’s specific force, angular rate, and sometimes the orientation. The Rev Control Hub contains an IMU that is a 9-axis IMU that is a combination of a Gyroscope, Accelerometer, and Magnetometer. A Gyroscope is a device that reports the angular orientation of an object in 3 dimensions. An Accelerometer is a device that reports the acceleration of an object in 3 dimensions. Acceleration can be thought of as the rate of change of speed at any given instant. A Magnetometer is a device that measures the strength of magnetic fields in 3 axes. This can be used as a compass to gain the orientation of a robot relative to the poles of the Earth, an absolute measurement.

In FIRST Tech Challenge, the Rev Control Hub contains an IMU (either the BNO055 or BHI260AP) and the FTC SDK provides functions that let you get the orientation of the hub as the robot moves around. For this program we will use the IMU as a Gyroscope and Accelerometer. In general, using the IMU as a Magnetometer is not recommended as the robot and field have metal parts that can interfere with compass readings.

Using the IMU in a Program

In general, the way to use the IMU is to initialize the IMU during the program’s initialization step. It takes a second or two for the IMU to perform it’s internal calibration/initialization. Then you typically just read the orientation or other values as the program is running. If needed, you can reset the IMU as the program is running.

Copy the SensorIMUOrthogonal_Blocks program

Unlike other tutorial pages, you can just copy the program and use it. We’ll explain the program in some detail.

Right click and select Download on GitHub file SensorIMUOrthogonal_Blocks.blk.

You can then connect to your robot and ‘upload’ the file to your robot, give the program the name SensorIMUOrthogonal_Blocks. Open this program in Blocks and you can see it’s a very long program (click here to see the .png file of the program).

There’s a IMU initialize block that requires as input the orientation of the Rev Control Hub. You should edit the program you just created and modify the logoFacingDirection and usbFacingDirection values to match your robot.

Note: this program assumes you have mounted the Rev Control Hub so that it is at right angles (90 degrees) to the robot and/or the ground. See the robot picture above where the Rev Control Hub is parallel to the robot and the ground. If you have mounted the robot at an angle like 45 degrees then you will have to calculate an orientation to use when initializing the IMU. See the SensorImuNonOrthogonal.java sample, there’s a corresponding block to initialize the IMU using angles.

The possible directions for the logo and usb port are: FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. For the Pushbot robot the values required are UP and RIGHT.

When you set the directions, you should also update the text value for the telemetry line that will display the Hub Orientation. In the Java program, they use variables to set the facing directions so they can re-use them in the Telemetry, but in Blocks it does not appear that you can create a variable of the facing directions.

This program is actually a Teleop program. While it is running it checks the gamepad for a Y button press and if Y is pressed the IMU Yaw is Reset by using the imu.resetYaw block.

Yaw is the Z axis of the orientation reading of the IMU. If the IMU is initialized with the proper control hub orientation, then the Z axis represents a Heading direction with a value of zero indicating forward.

Inside the opmode is active loop the program uses an imu.RobotYawPitchRollAngles block to get the IMU orientation. It then uses an imu.getRobotAngularVelocity to get the angular acceleration values.

After getting the current values, the program then uses telemetry to display the three orientation values and the three angular velocity values. The program then loops to get new orientation and velocity values.

With the correct orientation parameters selected, pitch/roll/yaw should act as follows:

• Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
• Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
• Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)

So if you rotate the robot Counter Clockwise from it’s starting position the Yaw (Heading) value should go from zero to a positive value.

Run the program now and verify the Pitch/Roll/Yaw values INCREASE when you move the robot as indicated above. If the values do not increase when you move the robot as indicated you should check that the control hub Logo and USB facing directions are correct.

While the program is running you should see the orientation (in degrees) displayed on the driver station, and the angular velocity displayed in degrees per second. Likely you’ll only use the IMU orientation values in your autonomous programs, and probably only the Yaw or heading value.

Next Steps

Now that you’ve seen what the IMU can do, let’s try using the IMU to control driving during the autonomous period.

Check out the other Blocks sample programs:

Blocks Sample Programs

Getting Help

It is often possible to use Google (or other search engine) to get help or solve problems. There are lots of resources online. If you’re still stuck you can ask for help here.