Java Orientation类代码示例

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

import org.firstinspires.ftc.robotcore.external.navigation.Orientation; //导入依赖的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.Orientation; //导入依赖的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.Orientation; //导入依赖的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.Orientation; //导入依赖的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.Orientation; //导入依赖的package包/类
@Override
public void runPeriodic(double elapsedTime)
{
    double startTime;

    startTime = TrcUtil.getCurrentTime();
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(vuforia.getTarget(0));
    dashboard.displayPrintf(1, "ElapseTime: getTarget=%f", TrcUtil.getCurrentTime() - startTime);

    if (vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        startTime = TrcUtil.getCurrentTime();
        VectorF pos = getVuMarkPosition();
        Orientation orientation = getVuMarkOrientation();
        dashboard.displayPrintf(2, "ElapseTime: getVuMarkInfo=%f", TrcUtil.getCurrentTime() - startTime);

        dashboard.displayPrintf(3, "%s: x=%6.2f,y=%6.2f,z=%6.2f",
                vuMark.toString(), pos.get(0)/MM_PER_INCH, pos.get(1)/MM_PER_INCH, pos.get(2)/MM_PER_INCH);
        dashboard.displayPrintf(4, "%s: xRot=%6.2f,yRot=%6.2f,zRot=%6.2f",
                vuMark.toString(), orientation.firstAngle, orientation.secondAngle, orientation.thirdAngle);
    }

    if (vuMark != prevVuMark)
    {
        String sentence = null;

        if (vuMark != RelicRecoveryVuMark.UNKNOWN)
        {
            sentence = String.format("%s is %s.", vuMark.toString(), "in view");
        }
        else if (prevVuMark != null)
        {
            sentence = String.format("%s is %s.", prevVuMark.toString(), "out of view");
        }

        if (sentence != null)
        {
            dashboard.displayPrintf(5, sentence);
            if (textToSpeech != null)
            {
                //
                // ZTE phones are on KitKat and running level 19 APIs, so we need to use the deprecated version
                // to be compatible with it.
                //
                textToSpeech.speak(sentence, TextToSpeech.QUEUE_FLUSH, null);
            }
        }

        prevVuMark = vuMark;
    }
}
 

import org.firstinspires.ftc.robotcore.external.navigation.Orientation; //导入依赖的package包/类
/**
 * This method creates a location matrix that can be used to relocate an object to its final location by rotating
 * and translating the object from the origin of the field. It is doing the operation in the order of the
 * parameters. In other words, it will first rotate the object on the X-axis, then rotate on the Y-axis, then
 * rotate on the Z-axis, then translate on the X-axis, then translate on the Y-axis and finally translate on the
 * Z-axis.
 *
 * @param rotateX specifies rotation on the X-axis.
 * @param rotateY specifies rotation on the Y-axis.
 * @param rotateZ specifies rotation on the Z-axis.
 * @param translateX specifies translation on the X-axis.
 * @param translateY specifies translation on the Y-axis.
 * @param translateZ specifies translation on the Z-axis.
 * @return returns the location matrix.
 */
public OpenGLMatrix locationMatrix(
        float rotateX, float rotateY, float rotateZ, float translateX, float translateY, float translateZ)
{
    return OpenGLMatrix.translation(translateX, translateY, translateZ)
            .multiplied(Orientation.getRotationMatrix(
                    AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, rotateX, rotateY, rotateZ));
}
 

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

    }