import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZYX);
                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.AxesOrder; //导入依赖的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().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZYX);
                }
            });

        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);
                    }
                });
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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.AxesOrder; //导入依赖的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);
                    }
                });
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; //导入依赖的package包/类
@Override public void runOpMode() throws InterruptedException {
    // Get a reference to a Modern Robotics GyroSensor object. We use several interfaces
    // on this object to illustrate which interfaces support which functionality.
    navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx");
    gyro = (IntegratingGyroscope)navxMicro;
    // If you're only interested int the IntegratingGyroscope interface, the following will suffice.
    // gyro = hardwareMap.get(IntegratingGyroscope.class, "navx");

    // The gyro automatically starts calibrating. This takes a few seconds.
    telemetry.log().add("Gyro Calibrating. Do Not Move!");

    // Wait until the gyro calibration is complete
    timer.reset();
    while (navxMicro.isCalibrating())  {
        telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
        telemetry.update();
        Thread.sleep(50);
    }
    telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
    telemetry.clear(); telemetry.update();

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

    while (opModeIsActive()) {

        // Read dimensionalized data from the gyro. This gyro can report angular velocities
        // about all three axes. Additionally, it internally integrates the Z axis to
        // be able to report an absolute angular Z orientation.
        AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES);
        Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);

        telemetry.addLine()
            .addData("dx", formatRate(rates.xRotationRate))
            .addData("dy", formatRate(rates.yRotationRate))
            .addData("dz", "%s deg/s", formatRate(rates.zRotationRate));

        telemetry.addLine()
            .addData("heading", formatAngle(angles.angleUnit, angles.firstAngle))
            .addData("roll", formatAngle(angles.angleUnit, angles.secondAngle))
            .addData("pitch", "%s deg", formatAngle(angles.angleUnit, angles.thirdAngle));
        telemetry.update();

        idle(); // Always call idle() at the bottom of your while(opModeIsActive()) loop
    }
}
 

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

  boolean lastResetState = false;
  boolean curResetState  = false;

  // Get a reference to a Modern Robotics gyro object. We use several interfaces
  // on this object to illustrate which interfaces support which functionality.
  modernRoboticsI2cGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro");
  gyro = (IntegratingGyroscope)modernRoboticsI2cGyro;
  // If you're only interested int the IntegratingGyroscope interface, the following will suffice.
  // gyro = hardwareMap.get(IntegratingGyroscope.class, "gyro");
  // A similar approach will work for the Gyroscope interface, if that's all you need.

  // Start calibrating the gyro. This takes a few seconds and is worth performing
  // during the initialization phase at the start of each opMode.
  telemetry.log().add("Gyro Calibrating. Do Not Move!");
  modernRoboticsI2cGyro.calibrate();

  // Wait until the gyro calibration is complete
  timer.reset();
  while (!isStopRequested() && modernRoboticsI2cGyro.isCalibrating())  {
    telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
    telemetry.update();
    sleep(50);
  }

  telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
  telemetry.clear(); telemetry.update();

  // Wait for the start button to be pressed
  waitForStart();
  telemetry.log().clear();
  telemetry.log().add("Press A & B to reset heading");

  // Loop until we're asked to stop
  while (opModeIsActive())  {

    // If the A and B buttons are pressed just now, reset Z heading.
    curResetState = (gamepad1.a && gamepad1.b);
    if (curResetState && !lastResetState) {
      modernRoboticsI2cGyro.resetZAxisIntegrator();
    }
    lastResetState = curResetState;

    // The raw() methods report the angular rate of change about each of the
    // three axes directly as reported by the underlying sensor IC.
    int rawX = modernRoboticsI2cGyro.rawX();
    int rawY = modernRoboticsI2cGyro.rawY();
    int rawZ = modernRoboticsI2cGyro.rawZ();
    int heading = modernRoboticsI2cGyro.getHeading();
    int integratedZ = modernRoboticsI2cGyro.getIntegratedZValue();

    // Read dimensionalized data from the gyro. This gyro can report angular velocities
    // about all three axes. Additionally, it internally integrates the Z axis to
    // be able to report an absolute angular Z orientation.
    AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES);
    float zAngle = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle;

    // Read administrative information from the gyro
    int zAxisOffset = modernRoboticsI2cGyro.getZAxisOffset();
    int zAxisScalingCoefficient = modernRoboticsI2cGyro.getZAxisScalingCoefficient();

    telemetry.addLine()
      .addData("dx", formatRate(rates.xRotationRate))
      .addData("dy", formatRate(rates.yRotationRate))
      .addData("dz", "%s deg/s", formatRate(rates.zRotationRate));
    telemetry.addData("angle", "%s deg", formatFloat(zAngle));
    telemetry.addData("heading", "%3d deg", heading);
    telemetry.addData("integrated Z", "%3d", integratedZ);
    telemetry.addLine()
      .addData("rawX", formatRaw(rawX))
      .addData("rawY", formatRaw(rawY))
      .addData("rawZ", formatRaw(rawZ));
    telemetry.addLine().addData("z offset", zAxisOffset).addData("z coeff", zAxisScalingCoefficient);
    telemetry.update();
  }
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; //导入依赖的package包/类
public  OpenGLMatrix createMatrix(float x,float y,float z,float u,float v,float w)
{
	return OpenGLMatrix.translation(x,y,z).
		multiplied(Orientation.getRotationMatrix(
			AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, u,v,w));
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; //导入依赖的package包/类
public double getAngle(){
    Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
    return angles.firstAngle;
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; //导入依赖的package包/类
public OpenGLMatrix createMatrix(float x, float y, float z, float u, float v, float w)
{
    return OpenGLMatrix.translation(x,y,z).
            multiplied(Orientation.getRotationMatrix(
                    AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, u,v,w));
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; //导入依赖的package包/类
@Override
public void loop(){
    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;
    }
    double x = gamepad1.right_stick_x;
    double y = gamepad1.right_stick_y;
    double c = gamepad1.left_trigger-gamepad1.right_trigger;
    motor1.setPower(Range.clip(y-x+c, -1, 1));
    motor2.setPower(Range.clip(y+x-c, -1, 1));
    motor3.setPower(Range.clip(y+x+c, -1, 1));
    motor4.setPower(Range.clip(y-x-c, -1, 1));

    if (gamepad2.right_trigger > 0) {
        leftcr.setPower(1);
        rightcr.setPower(1);
    }else if(gamepad2.left_trigger > 0) {
        leftcr.setPower(-1);
        rightcr.setPower(-1);
    }else {
        leftcr.setPower(0.1);
        rightcr.setPower(0);
    }
    if(gamepad2.x) {
        glyph.setPosition(1);
    }
    else if(gamepad2.a) {
        glyph.setPosition(0);
    }
    else if(gamepad2.b) {
        glyph.setPosition(-1);
    }

    telemetry.addData("motor 1 pos", motor1.getCurrentPosition());
    telemetry.addData("motor 2 pos", motor2.getCurrentPosition());
    telemetry.addData("motor 3 pos", motor3.getCurrentPosition());
    telemetry.addData("motor 4 pos", motor4.getCurrentPosition());
    telemetry.addData("jewel servo pos", jewelcr.getPower());
    updateLocation();
    telemetry.addData("Last Known Location", formatMatrix(lastKnownLocation));

    telemetry.addData("Red",color1.red());
    telemetry.addData("Blue",color1.blue());

    telemetry.update();
}
 

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; //导入依赖的package包/类
public OpenGLMatrix createMatrix(float x,float y,float z,float u,float v,float w)
{
    return OpenGLMatrix.translation(x,y,z).multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, u,v,w));
}