Measure Rotation with the Gyroscope

During auto, and also in teleop, it is sometimes helpful to know what direction the robot is facing. The control hub has a built-in gyroscope that can provide that information.

Go into the control hub robot configuration under I2C, port 0, select the appropriate IMU and name it imu. Add the following to your imports:

import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;

We also need to and create an instance variable for our gyroscope, as well as some global variables at the beginning of our class:

private IMU myImu;
private YawPitchRollAngles imuAngles;
private Double heading, headingRad;

Next we also need to get a reference to our gyroscope out of the hardware map, define how the control hub is oriented, initialize our gyroscope and set the heading to zero in our init method:

myImu = hardwareMap.get(IMU.class, "imu");
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; 
//**FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT
RevHubOrientationOnRobot.UsbFacingDirection  usbDirection  = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; 
//**FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
myImu.initialize(new IMU.Parameters(orientationOnRobot));
myImu.resetYaw();  //zeros Yaw angle (heading)

You will need to pick the appropriate directions for the logo and usb based on how the control hub is on your robot.

Create a new method for our gyroscope, and also call our gyroscope method in our loop method. Inside our gyroscope method add:

imuAngles = myImu.getRobotYawPitchRollAngles();
heading = imuAngles.getYaw(AngleUnit.DEGREES);
headingRad = imuAngles.getYaw(AngleUnit.RADIANS);

Add the heading and headingRad to our telemetry output.

Compile and run your code. As you drive your robot around, you should see the values of the heading on your Driver Hub change. Which way is the angle positive? What happens when the robot turns more than 180 degrees?