Robot.java

package org.firstinspires.ftc.teamcode.Skystone;


import android.os.SystemClock;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;

import org.firstinspires.ftc.robotcore.external.Telemetry;
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.DistanceUnit;
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.Timer;

import java.util.Timer;

import static org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit.CM;
import static org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit.MM;

public class Robot {
    //Drive Motors
    public DcMotor fLeft;
    public DcMotor fRight;
    public DcMotor bLeft;
    public DcMotor bRight;

    //imu
    private BNO055IMU imu;
    private Orientation angles;
    private Position position;

    //Inherited classes from Op Mode
    private Telemetry telemetry;
    private HardwareMap hardwareMap;
    private LinearOpMode linearOpMode;

    //PID (concept only)

    // TODO: move the position data here
    // private int xPosGlobal;
    // ...


    public Robot(HardwareMap hardwareMap, Telemetry telemetry, LinearOpMode linearOpMode){
        this.telemetry = telemetry;
        this.hardwareMap = hardwareMap;
        this.linearOpMode = linearOpMode;

        //config names need to match configs on the phone
        //Map drive motors
        fLeft = hardwareMap.dcMotor.get("fLeft");
        fRight = hardwareMap.dcMotor.get("fRight");
        bLeft = hardwareMap.dcMotor.get("bLeft");
        bRight = hardwareMap.dcMotor.get("bRight");

        //Set direction of drive motors
        fLeft.setDirection(DcMotor.Direction.FORWARD);
        fRight.setDirection(DcMotor.Direction.REVERSE);
        bLeft.setDirection(DcMotor.Direction.FORWARD);
        bRight.setDirection(DcMotor.Direction.REVERSE);
    }

    public void setDrivePower(double power){
        fLeft.setPower(power);
        fRight.setPower(power);
        bLeft.setPower(power);
        bRight.setPower(power);
    }

    public void intializeIMU(){
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
        parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
        parameters.loggingEnabled      = true;
        parameters.loggingTag          = "IMU";
        parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();

        imu = hardwareMap.get(BNO055IMU.class, "imu");
        imu.initialize(parameters);
        // TODO: I doubt you want to call startAccelerationIntegration()
        // the information it produces is useless and it secretly reads from another thread, potentially slowing your loops
        imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);

        angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.YXZ, AngleUnit.DEGREES);
    }

    public void resetEncoders(){
        fLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        fRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        bLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        bRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    }

    public void changeRunModeToUsingEncoder(){
        fLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        fRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        bLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        bRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    }

    public void setMotorMode(DcMotor.RunMode runMode){
        fLeft.setMode(runMode);
        fRight.setMode(runMode);
        bLeft.setMode(runMode);
        bRight.setMode(runMode);
    }

	public double[] getPosUsingIncrements() {
        // TODO: you can remove this; the odometry equations don't have a time dependence
        long startTime = SystemClock.elapsedRealtime();

        double[] positions = new double[3];
        final double radius = 2;
        final double encoderPerRevolution = 537.6;
        final double l = 7;
        final double w = 6.5;

        // TODO: again, move these to the robot scope
        double xPosGlobal = 0;
        double yPosGlobal = 0;
        double angleGlobal = 0;

        // find robot position
        double fl = 2 * Math.PI * fLeft.getCurrentPosition() / encoderPerRevolution; //radians each motor has travelled
        double fr = 2 * Math.PI * fRight.getCurrentPosition() / encoderPerRevolution;
        double bl = 2 * Math.PI * bLeft.getCurrentPosition() / encoderPerRevolution;
        double br = 2 * Math.PI * bRight.getCurrentPosition() / encoderPerRevolution;

        double xPosRobot = radius/4 * (fl + bl + br + fr);
        double yPosRobot = radius/4 * (-fl + bl - br + fr);
        double angleRobot = radius/4 *(-fl/(l+w) - bl/(l+w) + br/(l+w) + fr/(l+w));
        // TODO: since you use this angle for further computations, I wouldn't convert to degrees
        // Java requires radians for Math.cos(), Math.sin(), etc.
        angleRobot = angleRobot * 180/(2*Math.PI);

        //converting to global frame
        // TODO: remove this lopp
        // you want this code to run exactly once per loop and the op mode already does that
        while(linearOpMode.opModeIsActive()){

            long elapsedTime = SystemClock.elapsedRealtime() - startTime;

            //these variables have random names, used to make typing xPosGlobal and yPosGlobal easier
            // TODO: because fl, fr, bl, and br are all *deltas*, xPosRobot, yPosRobot, angleRobot are already deltas
            // therefore you don't need to multiply by elapsedTime (you don't need elapsedTime at all!)
            double angleDeltaRobot = angleRobot * elapsedTime;
            double xDeltaRobot = xPosRobot * elapsedTime;
            double yDeltaRobot = yPosRobot * elapsedTime;

            //finding global position
            // TODO: I don't really have the bandwidth to check this; I recommend splitting the two matrix multiplies into two sets of computations
            // I intentionally split the matrices for this exact purpose
            xPosGlobal += (Math.cos(angleGlobal) * Math.sin(angleDeltaRobot) - (Math.cos(angleDeltaRobot) - 1) * Math.sin(angleGlobal)) * xDeltaRobot / angleDeltaRobot + (Math.cos(angleGlobal) * (Math.cos(angleDeltaRobot) - 1) - Math.sin(angleGlobal) * Math.sin(angleDeltaRobot)) * yDeltaRobot / angleDeltaRobot;
            yPosGlobal += ((Math.cos(angleDeltaRobot) - 1) * Math.sin(angleGlobal) + (Math.cos(angleGlobal)) * Math.sin(angleDeltaRobot)) * yDeltaRobot / angleDeltaRobot + (Math.cos(angleGlobal) * (Math.cos(angleDeltaRobot) - 1) + Math.sin(angleGlobal) * Math.sin(angleDeltaRobot)) * xDeltaRobot / angleDeltaRobot;
            angleGlobal += angleDeltaRobot;

            // TODO: minor point but you can just do
            // return new double[] { xPosGlobal, yPosGlobal, angleGlobal };
            // or better yet make this method void and add getters for the global position
            //inserting into return array
            positions[0] += xPosGlobal;
            positions[1] += yPosGlobal;
            positions[2] += angleGlobal;

            // TODO: you shouldn't reset encoders in a loop as it wastes time
            // Instead just store the last positions yourself and compute the deltas manually
            resetEncoders();
            // TODO: there's really no reason to sleep with REV hardware since the calls are blocking
            // Sleeping makes your loops slower and hurts performance
            linearOpMode.sleep(10);
        }
        return positions;
    }
}