Turn Class

package org.firstinspires.ftc.teamcode;

import android.graphics.drawable.GradientDrawable;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
import com.qualcomm.hardware.lynx.commands.core.LynxGetMotorPIDControlLoopCoefficientsCommand;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.PIDCoefficients;

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 java.util.concurrent.TimeUnit;

public class Turn
{
    LinearOpMode opMode;
    public DcMotor BDL;
    public DcMotor BDR;
    public DcMotor FDR;
    public DcMotor FDL;
    BNO055IMU imu;
    Orientation orientation;
    double current;


    public Turn(LinearOpMode ahwMap)
    {
        this.opMode = ahwMap;
        BDL = ahwMap.hardwareMap.dcMotor.get("BDL");
        BDR = ahwMap.hardwareMap.dcMotor.get("BDR");
        FDL = ahwMap.hardwareMap.dcMotor.get("FDL");
        FDR = ahwMap.hardwareMap.dcMotor.get("FDR");
        imu = ahwMap.hardwareMap.get(BNO055IMU.class, "imu");
        BDL.setDirection(DcMotor.Direction.REVERSE);
        FDL.setDirection(DcMotor.Direction.REVERSE);

        BDL.setPower(0);
        BDR.setPower(0);
        FDL.setPower(0);
        FDR.setPower(0);

        // Set all otors to run without encoders.
        // May want to use RUN_USING_ENCODERS if encoders are installed.
        FDR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        FDL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        BDR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        BDL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

        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();

        // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
        // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
        // and named "imu".
        imu.initialize(parameters);
    }

    public void resetAngle()
    {
        orientation = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX , AngleUnit.DEGREES);
        current = 0;
    }

    public void IMUTurning (double target , double speed)
    {
        if(target > current)
        {
            opMode.telemetry.addData("toGo", current - target);
            while ((current + target) - current > 2 && opMode.opModeIsActive())
            {
                orientation = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
                current = orientation.firstAngle;
                opMode.telemetry.addData("toGo", target - current);
                opMode.telemetry.update();
                FDR.setPower(speed);
                FDL.setPower(-speed);
                BDR.setPower(-speed);
                BDL.setPower(speed);
                opMode.telemetry.update();
            }
        }
        else
        {
            opMode.telemetry.addData("toGo", target - current);
            while (current - (target + current) > 2 && opMode.opModeIsActive())
            {
                orientation = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
                current = orientation.firstAngle; //getHeading
                opMode.telemetry.addData("toGo", current - target);
                opMode.telemetry.update();
                FDR.setPower(-speed);
                FDL.setPower(speed);
                BDR.setPower(speed);
                BDL.setPower(-speed);
                opMode.telemetry.update();
            }
        }
        resetAngle();
    }
}