import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
public Orientation getVuMarkOrientation()
{
    Orientation targetAngle = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetAngle = Orientation.getOrientation(
                    pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
            robot.tracer.traceInfo("TargetRot", "%s: xRot=%6.2f, yRot=%6.2f, zRot=%6.2f",
                    vuMark.toString(),
                    targetAngle.firstAngle, targetAngle.secondAngle, targetAngle.thirdAngle);
        }
    }

    return targetAngle;
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
public void updateLocation(){
    OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicVuMark.getListener()).getPose();
    telemetry.addData("Pose", format(pose));

    /* We further illustrate how to decompose the pose into useful rotational and
    * translational components */
    if (pose != null) {
        VectorF trans = pose.getTranslation();
        Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);

        // Extract the X, Y, and Z components of the offset of the target relative to the robot
        tX = trans.get(0);
        tY = trans.get(1);
        tZ = trans.get(2);

        // Extract the rotational components of the target relative to the robot
        rX = rot.firstAngle;
        rY = rot.secondAngle;
        rZ = rot.thirdAngle;
        //Z is forward-backward
        //x is sideways
    }
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
private Orientation getVuMarkOrientation()
{
    Orientation targetAngle = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetAngle = Orientation.getOrientation(
                    pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
        }
    }

    return targetAngle;
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
@Override public void runOpMode() {

        // Set up the parameters with which we will use our IMU. Note that integration
        // algorithm here just reports accelerations to the logcat log; it doesn't actually
        // provide positional information.
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
        parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        parameters.calibrationDataFile = "AdafruitIMUCalibration.json"; // see the calibration sample opmode
        parameters.loggingEnabled      = true;
        parameters.loggingTag          = "IMU";
        parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();

        // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
        // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
        // and named "imu".
        imu = hardwareMap.get(BNO055IMU.class, "imu");
        imu.initialize(parameters);

        // Set up our telemetry dashboard
        composeTelemetry();

        // Wait until we're told to go
        waitForStart();

        // Start the logging of measured acceleration
        imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);

        // Loop and update the dashboard
        while (opModeIsActive()) {
            telemetry.update();
        }
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
@Override public void runOpMode() {

        // Set up the parameters with which we will use our IMU. Note that integration
        // algorithm here just reports accelerations to the logcat log; it doesn't actually
        // provide positional information.
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
        parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
        parameters.loggingEnabled      = true;
        parameters.loggingTag          = "IMU";
        parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();

        // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
        // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
        // and named "imu".
        imu = hardwareMap.get(BNO055IMU.class, "imu");
        imu.initialize(parameters);

        // Set up our telemetry dashboard
        composeTelemetry();

        // Wait until we're told to go
        waitForStart();

        // Start the logging of measured acceleration
        imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);

        // Loop and update the dashboard
        while (opModeIsActive()) {
            telemetry.update();
        }
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
@Override public void runOpMode() throws InterruptedException {

        // Get a reference to the gyroscope from the hardware map
        gyroscope = hardwareMap.get(Gyroscope.class, "gyro");

        // Get a reference to the *implementation* of the gyroscope on the HiTechnic sensor.
        // Usually, you won't need to examine internal implementation details in this way, but
        // we do so to illustrate aspects of what is going on inside the sensor.
        hiTechnicNxtGyroSensor = hardwareMap.get(HiTechnicNxtGyroSensor.class, "gyro");
        // Alternately, we could have cast: hiTechnicNxtGyroSensor = (HiTechnicNxtGyroSensor)gyro;

        // Optionally, calibrate the gyro to establish a good value for its "zero deg/s" bias
        // voltage value. Calibration is not entirely necessary, as the default bias voltage
        // usually does a reasonable job. While calibration is performed, the gyro must remain
        // motionless. Note that for this gyro sensor, calibration data is not persistently
        // written to EEPROM, but rather should be performed each run.
        telemetry.log().add("calibrating...");
        hiTechnicNxtGyroSensor.calibrate(3000, 100);
        telemetry.log().add("...done...waiting for start...");

        // wait for the start button to be pressed.
        waitForStart();
        telemetry.log().clear();

        // loop until the opmode has been asked to stop
        while (opModeIsActive()) {

            double raw = hiTechnicNxtGyroSensor.readRawVoltage();
            double bias = hiTechnicNxtGyroSensor.getBiasVoltage();

            telemetry.addData("rate", "%.4f deg/s",      gyroscope.getAngularVelocity(AngleUnit.DEGREES).zRotationRate);
            telemetry.addData("raw ", "%.4fv",           raw);
            telemetry.addData("bias", "%.4fv",           bias);
            telemetry.addData("volts", "%.4fv",          raw-bias);
            telemetry.addData("deg/s/v", "%.4f deg/s/v", hiTechnicNxtGyroSensor.getDefaultDegreesPerSecondPerVolt());

            telemetry.update();
            idle();
        }
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
public void runOpMode() throws InterruptedException
{
	setupVuforia();
	
	lastKnownLocation = createMatrix(0,0,0,0,0,0);
	
	waitForStart();
	
	visionTargets.activate();
	while(opModeIsActive())
	{
		OpenGLMatrix latestLocation = listener.getUpdatedRobotLocation();
		vuMark = RelicRecoveryVuMark.from(relicVuMark);
	
		if(latestLocation !=null)
		{lastKnownLocation = latestLocation;}
			
		float[] coordinates = lastKnownLocation.getTranslation().getData();
		
		robotX = coordinates[0];
		robotY = coordinates[1];
		robotAngle = Orientation.getOrientation(lastKnownLocation, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
           RelicRecoveryVuMark key = vuMark;
		if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
               telemetry.addData("Navi", "%s visible", vuMark);
           }else{
               telemetry.addData("Navi", "not visible");
               telemetry.addData("Navi Sees:", vuMark);
           }
		telemetry.addData("Last Known Location", formatMatrix(lastKnownLocation));
		telemetry.addData("key",key.toString());
		telemetry.update();
	}
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
@Override
public void init() throws InterruptedException {
    motor1 = robot.hardwareMap.dcMotor.get("motor1");
    motor2 = robot.hardwareMap.dcMotor.get("motor2");
    motor3 = robot.hardwareMap.dcMotor.get("motor3");
    motor4 = robot.hardwareMap.dcMotor.get("motor4");

    motor1.setDirection(DcMotor.Direction.FORWARD);
    motor2.setDirection(DcMotor.Direction.REVERSE);
    motor3.setDirection(DcMotor.Direction.FORWARD);
    motor4.setDirection(DcMotor.Direction.REVERSE);

    modeEncoders();
    BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();

    parameters.mode                = BNO055IMU.SensorMode.IMU;
    parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
    parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
    parameters.loggingEnabled      = false;

    imu = robot.hardwareMap.get(BNO055IMU.class, "imu");

    imu.initialize(parameters);

    robot.telemetry.addData("Mode", "calibrating...");
    robot.telemetry.update();

    while(!imu.isGyroCalibrated()){
        sleep(50);
    }

    robot.telemetry.addData("Mode", "waiting for start");
    robot.telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());
    robot.telemetry.addData("Angle", getAngle());
    robot.telemetry.update();

}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
public void runOpMode() throws InterruptedException
{
    setupVuforia();

    lastKnownLocation = createMatrix(0,0,0,0,0,0);

    waitForStart();

    visionTargets.activate();
    while(opModeIsActive())
    {
        OpenGLMatrix latestLocation = listener.getUpdatedRobotLocation();
        vuMark = RelicRecoveryVuMark.from(relicVuMark);

        if(latestLocation !=null)
        {lastKnownLocation = latestLocation;}

        float[] coordinates = lastKnownLocation.getTranslation().getData();

        robotX = coordinates[0];
        robotY = coordinates[1];
        robotAngle = Orientation.getOrientation(lastKnownLocation, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
        if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
            key = vuMark;
        }
    }
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
@Override public void runOpMode() throws InterruptedException {

        // Set up the parameters with which we will use our IMU. Note that integration
        // algorithm here just reports accelerations to the logcat log; it doesn't actually
        // provide positional information.
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
        parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        parameters.calibrationDataFile = "AdafruitIMUCalibration.json"; // see the calibration sample opmode
        parameters.loggingEnabled      = true;
        parameters.loggingTag          = "IMU";
        parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();

        // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
        // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
        // and named "imu".
        imu = hardwareMap.get(BNO055IMU.class, "imu");
        imu.initialize(parameters);

        // Set up our telemetry dashboard
        composeTelemetry();

        // Wait until we're told to go
        waitForStart();

        // Start the logging of measured acceleration
        imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);

        // Loop and update the dashboard
        while (opModeIsActive()) {
            telemetry.update();
            idle();
        }
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
@Override
public void runPeriodic(double elapsedTime)
{
    dashboard.displayPrintf(1, "Angle:x=%6.1f,y=%6.1f,z=%6.1f",
            imu.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle,
            imu.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).secondAngle,
            imu.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle);
    dashboard.displayPrintf(2, "Heading: x=%6.1f,y=%6.1f,z=%6.1f",
            imu.gyro.getXHeading().value,
            imu.gyro.getYHeading().value,
            imu.gyro.getZHeading().value);
    dashboard.displayPrintf(3, "TurnRate: x=%6.1f,y=%6.1f,z=%6.1f",
            imu.gyro.getXRotationRate().value,
            imu.gyro.getYRotationRate().value,
            imu.gyro.getZRotationRate().value);
    dashboard.displayPrintf(4, "Accel: x=%6.1f,y=%6.1f,z=%6.1f",
            imu.accel.getXAcceleration().value,
            imu.accel.getYAcceleration().value,
            imu.accel.getZAcceleration().value);
    dashboard.displayPrintf(5, "Vel: x=%6.1f,y=%6.1f,z=%6.1f",
            imu.accel.getXVelocity().value,
            imu.accel.getYVelocity().value,
            imu.accel.getZVelocity().value);
    dashboard.displayPrintf(6, "Dist: x=%6.1f,y=%6.1f,z=%6.1f",
            imu.accel.getXDistance().value,
            imu.accel.getYDistance().value,
            imu.accel.getZDistance().value);
    dashboard.displayPrintf(7, "Touch=%s", touchSensor.isActive());
    dashboard.displayPrintf(8, "Color=%x,rgb=%f/%f/%f",
            colorSensor.getRawData(0, FtcColorSensor.DataType.COLOR_NUMBER).value.intValue(),
            colorSensor.getRawData(0, FtcColorSensor.DataType.RED).value,
            colorSensor.getRawData(0, FtcColorSensor.DataType.GREEN).value,
            colorSensor.getRawData(0, FtcColorSensor.DataType.BLUE).value);
    dashboard.displayPrintf(9, "HSV=%f/%f/%f",
            colorSensor.getRawData(0, FtcColorSensor.DataType.HUE).value,
            colorSensor.getRawData(0, FtcColorSensor.DataType.SATURATION).value,
            colorSensor.getRawData(0, FtcColorSensor.DataType.VALUE).value);
    dashboard.displayPrintf(10, "Range=%f",
            rangeSensor.getRawData(0, FtcDistanceSensor.DataType.DISTANCE_INCH).value);
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
@Override public void runOpMode() throws InterruptedException {

        // Set up the parameters with which we will use our IMU. Note that integration
        // algorithm here just reports accelerations to the logcat log; it doesn't actually
        // provide positional information.
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
        parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        parameters.calibrationDataFile = "AdafruitIMUCalibration.json"; // see the calibration sample opmode
        parameters.loggingEnabled      = true;
        parameters.loggingTag          = "IMU";
        parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();

        // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
        // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
        // and named "imu".
        imu = hardwareMap.get(BNO055IMU.class, "imu");
        imu.initialize(parameters);

        // Set up our telemetry dashboard
        composeTelemetry();

        // Wait until we're told to go
        waitForStart();

        // Start the logging of measured acceleration
        imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);

        // Loop and update the dashboard
        while (opModeIsActive()) {
            telemetry.update();
        }
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
String formatAngle(AngleUnit angleUnit, double angle) {
    return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
String formatDegrees(double degrees){
    return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
/**
 * This method returns the raw data of the specified type for the x-axis.
 *
 * @param dataType specifies the data type.
 * @return raw data of the specified type for the x-axis.
 */
@Override
public SensorData<Double> getRawXData(DataType dataType)
{
    final String funcName = "getRawXData";
    double value = 0.0;
    long currTagId = FtcOpMode.getLoopCounter();

    if (dataType == DataType.ROTATION_RATE)
    {
        if (currTagId != turnRateTagId)
        {
            turnRateData = imu.getAngularVelocity();
            turnRateTagId = currTagId;
        }
        value = turnRateData.xRotationRate;
    }
    else if (dataType == DataType.HEADING)
    {
        if (currTagId != headingTagId)
        {
            if (USE_QUATERNION)
            {
                getEulerAngles(eulerAngles);
            }
            else
            {
                headingData = imu.getAngularOrientation(
                        AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
            }
            headingTagId = currTagId;
        }

        if (USE_QUATERNION)
        {
            value = eulerAngles[0];
        }
        else
        {
            value = headingData.firstAngle;
        }
    }
    SensorData<Double> data = new SensorData<>(TrcUtil.getCurrentTime(), value);

    if (debugEnabled)
    {
        dbgTrace.traceEnter(funcName, TrcDbgTrace.TraceLevel.API);
        dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API,
                "=(timestamp:%.3f,value:%f", data.timestamp, data.value);
    }

    return data;
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
/**
 * This method returns the raw data of the specified type for the y-axis.
 *
 * @param dataType specifies the data type.
 * @return raw data of the specified type for the y-axis.
 */
@Override
public SensorData<Double> getRawYData(DataType dataType)
{
    final String funcName = "getRawYData";
    double value = 0.0;
    long currTagId = FtcOpMode.getLoopCounter();

    if (dataType == DataType.ROTATION_RATE)
    {
        if (currTagId != turnRateTagId)
        {
            turnRateData = imu.getAngularVelocity();
            turnRateTagId = currTagId;
        }
        value = turnRateData.yRotationRate;
    }
    else if (dataType == DataType.HEADING)
    {
        if (currTagId != headingTagId)
        {
            if (USE_QUATERNION)
            {
                getEulerAngles(eulerAngles);
            }
            else
            {
                headingData = imu.getAngularOrientation(
                        AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
            }
            headingTagId = currTagId;
        }

        if (USE_QUATERNION)
        {
            value = eulerAngles[1];
        }
        else
        {
            value = headingData.secondAngle;
        }
    }
    SensorData<Double> data = new SensorData<>(TrcUtil.getCurrentTime(), value);

    if (debugEnabled)
    {
        dbgTrace.traceEnter(funcName, TrcDbgTrace.TraceLevel.API);
        dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API,
                "=(timestamp:%.3f,value:%f", data.timestamp, data.value);
    }

    return data;
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
/**
 * This method returns the raw data of the specified type for the z-axis.
 *
 * @param dataType specifies the data type.
 * @return raw data of the specified type for the z-axis.
 */
@Override
public SensorData<Double> getRawZData(DataType dataType)
{
    final String funcName = "getRawZData";
    double value = 0.0;
    long currTagId = FtcOpMode.getLoopCounter();

    if (dataType == DataType.ROTATION_RATE)
    {
        if (currTagId != turnRateTagId)
        {
            turnRateData = imu.getAngularVelocity();
            turnRateTagId = currTagId;
        }
        value = turnRateData.zRotationRate;
    }
    else if (dataType == DataType.HEADING)
    {
        if (currTagId != headingTagId)
        {
            if (USE_QUATERNION)
            {
                getEulerAngles(eulerAngles);
            }
            else
            {
                headingData = imu.getAngularOrientation(
                        AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
            }
            headingTagId = currTagId;
        }

        if (USE_QUATERNION)
        {
            value = -eulerAngles[2];
        }
        else
        {
            //
            // The Z-axis returns positive heading in the anticlockwise direction, so we must negate it for
            // our convention.
            //
            value = -headingData.thirdAngle;
        }
    }
    SensorData<Double> data = new SensorData<>(TrcUtil.getCurrentTime(), value);

    if (debugEnabled)
    {
        dbgTrace.traceEnter(funcName, TrcDbgTrace.TraceLevel.API);
        dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API,
                "=(timestamp:%.3f,value:%f", data.timestamp, data.value);
    }

    return data;
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
/**
 * Constructor: Creates an instance of the object.
 *
 * @param hardwareMap specifies the global hardware map.
 * @param instanceName specifies the instance name.
 */
public FtcBNO055Imu(HardwareMap hardwareMap, String instanceName)
{
    if (debugEnabled)
    {
        dbgTrace = new TrcDbgTrace(moduleName + "." + instanceName, tracingEnabled, traceLevel, msgLevel);
    }
    //
    // Initialize the BNO055 IMU.
    //
    BNO055IMU.Parameters imuParams = new BNO055IMU.Parameters();
    if (USE_QUATERNION)
    {
        imuParams.mode = BNO055IMU.SensorMode.IMU;
        imuParams.useExternalCrystal = true;
        imuParams.pitchMode = BNO055IMU.PitchMode.WINDOWS;
    }
    else
    {
        imuParams.calibrationDataFile = "BNO055IMUCalibration.json";
    }
    imuParams.angleUnit = BNO055IMU.AngleUnit.DEGREES;
    imuParams.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
    imuParams.loggingEnabled = true;
    imuParams.loggingTag = "IMU";
    imuParams.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
    imu = hardwareMap.get(BNO055IMU.class, instanceName);
    imu.initialize(imuParams);
    //
    // Create the gyro object of the IMU.
    // Note that the heading data on the z-axis is in Ordinal system with a range of -180 to 180 degrees.
    // So we need to initialize the Cartesian converter with the range values.
    //
    gyro = new Gyro(instanceName);
    //
    // Note:
    // We can convert only X (roll) and Z (yaw) axes to Cartesian.
    // For Y (pitch), when pointing upright, it will return 90-degree but pitching forward or backward will
    // yield the same decrement. So one can't tell if it is rotating forward or backward. This makes it
    // impossible to do the Cartesian conversion.
    //
    gyro.setXValueRange(-180.0, 180.0);
    gyro.setZValueRange(-180.0, 180.0);
    //
    // Create the accelerometer object of the IMU.
    //
    accel = new Accelerometer(instanceName);
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
void composeTelemetry() {

        // At the beginning of each telemetry update, grab a bunch of data
        // from the IMU that we will then display in separate lines.
        telemetry.addAction(new Runnable() { @Override public void run()
                {
                // Acquiring the angles is relatively expensive; we don't want
                // to do that in each of the three items that need that info, as that's
                // three times the necessary expense.
                angles   = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
                gravity  = imu.getGravity();
                }
            });

        telemetry.addLine()
            .addData("status", new Func<String>() {
                @Override public String value() {
                    return imu.getSystemStatus().toShortString();
                    }
                })
            .addData("calib", new Func<String>() {
                @Override public String value() {
                    return imu.getCalibrationStatus().toString();
                    }
                });

        telemetry.addLine()
            .addData("heading", new Func<String>() {
                @Override public String value() {
                    return formatAngle(angles.angleUnit, angles.firstAngle);
                    }
                })
            .addData("roll", new Func<String>() {
                @Override public String value() {
                    return formatAngle(angles.angleUnit, angles.secondAngle);
                    }
                })
            .addData("pitch", new Func<String>() {
                @Override public String value() {
                    return formatAngle(angles.angleUnit, angles.thirdAngle);
                    }
                });

        telemetry.addLine()
            .addData("grvty", new Func<String>() {
                @Override public String value() {
                    return gravity.toString();
                    }
                })
            .addData("mag", new Func<String>() {
                @Override public String value() {
                    return String.format(Locale.getDefault(), "%.3f",
                            Math.sqrt(gravity.xAccel*gravity.xAccel
                                    + gravity.yAccel*gravity.yAccel
                                    + gravity.zAccel*gravity.zAccel));
                    }
                });
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //导入依赖的package包/类
void composeTelemetry() {

        // At the beginning of each telemetry update, grab a bunch of data
        // from the IMU that we will then display in separate lines.
        telemetry.addAction(new Runnable() { @Override public void run()
                {
                // Acquiring the angles is relatively expensive; we don't want
                // to do that in each of the three items that need that info, as that's
                // three times the necessary expense.
                angles   = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
                }
            });

        telemetry.addLine()
            .addData("status", new Func<String>() {
                @Override public String value() {
                    return imu.getSystemStatus().toShortString();
                    }
                })
            .addData("calib", new Func<String>() {
                @Override public String value() {
                    return imu.getCalibrationStatus().toString();
                    }
                });

        telemetry.addLine()
            .addData("heading", new Func<String>() {
                @Override public String value() {
                    return formatAngle(angles.angleUnit, angles.firstAngle);
                    }
                })
            .addData("roll", new Func<String>() {
                @Override public String value() {
                    return formatAngle(angles.angleUnit, angles.secondAngle);
                    }
                })
            .addData("pitch", new Func<String>() {
                @Override public String value() {
                    return formatAngle(angles.angleUnit, angles.thirdAngle);
                    }
                });
    }