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

Example 1 with Orientation

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

the class ModernRoboticsI2cGyro method getAngularOrientation.

@Override
public Orientation getAngularOrientation(AxesReference reference, AxesOrder order, org.firstinspires.ftc.robotcore.external.navigation.AngleUnit angleUnit) {
    TimestampedData data = this.deviceClient.readTimeStamped(Register.INTEGRATED_Z_VALUE.bVal, 2);
    int integratedZ = TypeConversion.byteArrayToShort(data.data, ByteOrder.LITTLE_ENDIAN);
    float cartesian = AngleUnit.normalizeDegrees(degreesZFromIntegratedZ(integratedZ));
    return new Orientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES, cartesian, 0, 0, data.nanoTime).toAxesReference(reference).toAxesOrder(order).toAngleUnit(angleUnit);
}

Example 2 with Orientation

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

the class MatrixF method formatAsTransform.

/**
 * A simple utility that extracts positioning information from a transformation matrix
 * and formats it in a form palatable to a human being. This should only be invoked on
 * a matrix which is a transformation matrix.
 *
 * @param axesReference the reference frame of the angles to use in reporting the transformation
 * @param axesOrder     the order of the angles to use in reporting the transformation
 * @param unit          the angular unit to use in reporting the transformation
 * @return a description of the angles represented by this transformation matrix.
 * @see #formatAsTransform()
 * @see <a href="https://en.wikipedia.org/wiki/Transformation_matrix">Transformation Matrix</a>
 */
public String formatAsTransform(AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit) {
    /**
     * An easy way to understand what a transform does is to look at the location
     * to which it transforms the origin of the coordinate system. Calling getTranslation()
     * carries out an equivalent computation as it extracts the translational aspect.
     */
    VectorF translation = this.getTranslation();
    /**
     * Figure out in which direction we'd be looking after the transformation. Note that
     * the decomposition of a transformation into orientation angles can be subtle. See
     * {@link Orientation} for a full discussion.
     */
    Orientation orientation = Orientation.getOrientation(this, axesReference, axesOrder, unit);
    return String.format("%s %s", orientation.toString(), translation.toString());
}

Example 3 with Orientation

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

the class SensorKLNavxMicro method runOpMode.

@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();
        // Always call idle() at the bottom of your while(opModeIsActive()) loop
        idle();
    }
}

Example 4 with Orientation

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

the class ImageTransformProvider method run.

/**
 * Reads and records the pictograph type, relative distance, and relative rotation from the image
 * As well as filtering it through a data filer
 */
@Override
public void run() {
    VectorF translation;
    Orientation rotation;
    while (running) {
        if (!RelicRecoveryVuMark.from(template).equals(RelicRecoveryVuMark.UNKNOWN)) {
            OpenGLMatrix transform = ((VuforiaTrackableDefaultListener) template.getListener()).getPose();
            translation = transform.getTranslation();
            rotation = Orientation.getOrientation(transform, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
            xTrans.update(translation.get(0));
            yTrans.update(translation.get(1));
            zTrans.update(translation.get(2));
            xRot.update(rotation.firstAngle);
            yRot.update(rotation.secondAngle);
            zRot.update(rotation.thirdAngle);
            imageSeen = true;
            System.out.println(translation.get(0));
        } else
            imageSeen = false;
        try {
            Thread.sleep(UPDATE_INTERVAL);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }
}

Example 5 with Orientation

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

the class SensorKLNavxMicro method runOpMode.

@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();
        // Always call idle() at the bottom of your while(opModeIsActive()) loop
        idle();
    }
}