Examples with Orientation org.firstinspires.ftc.robotcore.external.navigation.Orientation used on opensource projects

Example 6 with Orientation

use of org.firstinspires.ftc.robotcore.external.navigation.Orientation in project FTC-2017 by FIRST-4030.

the class VuforiaFTC method track.

public void track() {
    if (!isRunning()) {
        return;
    }
    for (VuforiaTrackable trackable : targets) {
        // Per-target visibility (somewhat imaginary but still useful)
        targetVisible.put(trackable.getName(), ((VuforiaTrackableDefaultListener) trackable.getListener()).isVisible());
        // Angle to target, if available
        OpenGLMatrix newPose = ((VuforiaTrackableDefaultListener) trackable.getListener()).getPose();
        if (newPose != null) {
            Orientation poseOrientation = Orientation.getOrientation(newPose, AXES_REFERENCE, AxesOrder.XYZ, ANGLE_UNIT);
            targetAngle.put(trackable.getName(), (int) poseOrientation.secondAngle);
        }
        /*
             * Update the location and orientation track
             *
             * We poll for each trackable so this happens in the loop, but the overall tracking
             * is aggregated among all targets with a defined pose and location. The current
             * field of view will dictate the quality of the track and if one or more targets
             * are present they will be the primary basis for tracking but tracking persists
             * even when the view does not include a target, and is self-consistent when the
             * view includes multiple targets
             */
        OpenGLMatrix newLocation = ((VuforiaTrackableDefaultListener) trackable.getListener()).getUpdatedRobotLocation();
        if (newLocation != null) {
            // Extract our location from the matrix
            for (int i = 0; i < location.length; i++) {
                location[i] = (int) newLocation.get(i, 3);
            }
            // Calculate the orientation of our view
            Orientation newOrientation = Orientation.getOrientation(newLocation, AXES_REFERENCE, AxesOrder.XYZ, ANGLE_UNIT);
            orientation[0] = (int) newOrientation.firstAngle;
            orientation[1] = (int) newOrientation.secondAngle;
            orientation[2] = (int) newOrientation.thirdAngle;
            // Timestamp the update
            timestamp = System.currentTimeMillis();
        }
    }
}

Example 7 with Orientation

use of org.firstinspires.ftc.robotcore.external.navigation.Orientation in project FTC-2017 by FIRST-4030.

the class ConceptVuMarkIdentification method runOpMode.

@Override
public void runOpMode() {
    /*
         * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
         * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
         */
    int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
    VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
    // OR...  Do Not Activate the Camera Monitor View, to save power
    // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
    /*
         * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
         * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
         * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
         * web site at https://developer.vuforia.com/license-manager.
         *
         * Vuforia license keys are always 380 characters long, and look as if they contain mostly
         * random data. As an example, here is a example of a fragment of a valid key:
         *      ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
         * Once you've obtained a license key, copy the string from the Vuforia web site
         * and paste it in to your code onthe next line, between the double quotes.
         */
    parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX";
    /*
         * We also indicate which camera on the RC that we wish to use.
         * Here we chose the back (HiRes) camera (for greater range), but
         * for a competition robot, the front camera might be more convenient.
         */
    parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
    this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
    /**
     * Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
     * in this data set: all three of the VuMarks in the game were created from this one template,
     * but differ in their instance id information.
     * @see VuMarkInstanceId
     */
    VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
    VuforiaTrackable relicTemplate = relicTrackables.get(0);
    // can help in debugging; otherwise not necessary
    relicTemplate.setName("relicVuMarkTemplate");
    telemetry.addData(">", "Press Play to start");
    telemetry.update();
    waitForStart();
    relicTrackables.activate();
    while (opModeIsActive()) {
        /**
         * See if any of the instances of {@link relicTemplate} are currently visible.
         * {@link RelicRecoveryVuMark} is an enum which can have the following values:
         * UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
         * UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
         */
        RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
        if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
            /* Found an instance of the template. In the actual game, you will probably
                 * loop until this condition occurs, then move on to act accordingly depending
                 * on which VuMark was visible. */
            telemetry.addData("VuMark", "%s visible", vuMark);
            /* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
                 * it is perhaps unlikely that you will actually need to act on this pose information, but
                 * we illustrate it nevertheless, for completeness. */
            OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.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
                double tX = trans.get(0);
                double tY = trans.get(1);
                double tZ = trans.get(2);
                // Extract the rotational components of the target relative to the robot
                double rX = rot.firstAngle;
                double rY = rot.secondAngle;
                double rZ = rot.thirdAngle;
            }
        } else {
            telemetry.addData("VuMark", "not visible");
        }
        telemetry.update();
    }
}

Example 8 with Orientation

use of org.firstinspires.ftc.robotcore.external.navigation.Orientation in project robotcode by OutoftheBoxFTC.

the class NavxMicroNavigationSensor method getAngularOrientation.

@Override
public Orientation getAngularOrientation(AxesReference reference, AxesOrder order, org.firstinspires.ftc.robotcore.external.navigation.AngleUnit angleUnit) {
    TimestampedData data = this.deviceClient.readTimeStamped(Register.YAW_L.bVal, 3 * 2);
    // 
    float zDegrees = -shortToSignedHundredths(TypeConversion.byteArrayToShort(data.data, 0, ByteOrder.LITTLE_ENDIAN));
    float yDegrees = shortToSignedHundredths(TypeConversion.byteArrayToShort(data.data, 2, ByteOrder.LITTLE_ENDIAN));
    float xDegrees = shortToSignedHundredths(TypeConversion.byteArrayToShort(data.data, 4, ByteOrder.LITTLE_ENDIAN));
    // 
    return new Orientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES, zDegrees, yDegrees, xDegrees, data.nanoTime).toAxesReference(reference).toAxesOrder(order).toAngleUnit(angleUnit);
}

Example 9 with Orientation

use of org.firstinspires.ftc.robotcore.external.navigation.Orientation in project robotcode by OutoftheBoxFTC.

the class NavXGyroscopeProvider method run.

@Override
public void run() {
    while (!requestQuit) {
        setTimestamp(System.currentTimeMillis());
        Orientation angles = navxDevice.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
        setX(getX() + angles.firstAngle);
        setY(getZ() + angles.secondAngle);
        setZ(getZ() + angles.thirdAngle);
    }
    quit = true;
}

Example 10 with Orientation

use of org.firstinspires.ftc.robotcore.external.navigation.Orientation in project robotcode by OutoftheBoxFTC.

the class RevIMUGyroscopeProvider method update.

public void update() {
    Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
    setX(angles.thirdAngle);
    setY(angles.secondAngle);
    setZ(angles.firstAngle);
    setTimestamp(angles.acquisitionTime);
}