OrientationTracker

OrientationTracker.java

package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;

import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;

import java.util.Locale;

public class OrientationTracker extends MecanumDrive {

    // the program object representing the IMU sensor
    private BNO055IMU imu;

    /*
     * the current orientation and gravitational acceleration of the robot -- further information
     * really requires calibration (refer to the SensorBNO055IMUCalibration OpMode in
     * FtcRobotController/external.samples
     */
    private Orientation angles;
    private Acceleration gravity;

    // record the first direction the robot was headed
    private float initialHeading;

    @Override
    public void init() {
        // initialize the imu -- this takes time, so we do it first
        imu = hardwareMap.get(BNO055IMU.class, "imu");
        imu.initialize(imuConfig());

        // prepare automated telemetry updates (so we don't have to deal with them in loop())
        initTelemetry();

        // run the Mecanum.init() method, so we have motors
        super.init();
    }

    /**
     * This method is called when the start button is pressed on the Driver Station
     */
    @Override
    public void start() {
        // give the imu a starting point for calculating angles and gravity
        imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);
        initialHeading  = angles.firstAngle; // firstAngle is the Z-axis (per our configuration)
    }

    @Override
    public void loop() {
        super.loop();

        // post our current rotation amount to telemtry
        telemetry.addData("rotation since start", formatDegrees(angles.firstAngle - initialHeading));

        // post any updated data to the telemetry
        telemetry.update();
    }

    /*---------------------------------------------------------------------------------------------
     * All of the below methods are helpers that we use to make the methods above easier to read
     *--------------------------------------------------------------------------------------------*/

    /**
     * A helper function to generate the IMU configuration
     * @return a Parameters object to initialize the IMU
     */
    private BNO055IMU.Parameters imuConfig() {
        BNO055IMU.Parameters config = new BNO055IMU.Parameters();
        config.angleUnit = BNO055IMU.AngleUnit.DEGREES;
        config.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        config.loggingEnabled = true;
        config.loggingTag = "IMU";
        config.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
        return config;
    }

    /**
     * Initialize a separate thread to update angles and gravity independently of the loop() method,
     * and to post that information (nicely formatted) into telemetry
     */
    private void initTelemetry() {
        // update the current values of angles and gravity every time telemetry.update() is called
        telemetry.addAction(new Runnable() {
            @Override
            public void run() {
                angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
                gravity = imu.getGravity();
            }
        });

        // add a line to the telemetry tied to the IMU status and calibration (updated whenever telemetry.update() is called)
        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();
                    }
                });

        // add a line to the telemetry tied to the angles of the IMU (updated every time telemetry.update() is called)
        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);
                    }
                });

        // add a line to the telemetry tied to gravity sensed by the IMU (updated every time telemetry.update() is called)
        telemetry.addLine()
                .addData("gravity", 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));
                    }
                });
    }

    /*
     * A helper function to format angle information for pretty printing in telemetry
     */
    private String formatAngle(AngleUnit angleUnit, double angle) {
        return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
    }

    /*
     * A helper function to format degrees values for pretty printing in telemetry
     */
    private String formatDegrees(double degrees) {
        return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
    }

}