CurrentAuton

package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import java.util.List;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import com.qualcomm.hardware.bosch.BNO055IMU;

@Autonomous(name = "RussianSoy697Tensor")

public class TensorFlowAlonePractice extends OpMode {




//fellow comrades, please note that the encoder ticks below are just for reference, since they
//must be calculated later on
//this is for crater



//@Disa

        //Declare OpMode members
        private DcMotor upperLeftDrive = null;
        private DcMotor lowerLeftDrive = null;
        private DcMotor upperRightDrive = null;
        private DcMotor lowerRightDrive = null;
        private DcMotor hook = null;
        private Servo teamMarker = null;
        private DcMotor intakeArm = null;
        private BNO055IMU imu;
        private AngleUnit unit = AngleUnit.DEGREES;


        //these are example ratios and circumferences for the drive train,
        //change accordingly, btw for 2:1 it is 2, 80 tooth to 40 tooth
        //for gyro, i am assuming that the head of the robot is at 0 initially, and goes clockwise

        private double DriveGearRatio = 1.0 / 2.0;
        private double DriveCircumference = (1.97 * 2) * Math.PI;

        //not for the actual motor, but for the final gear
        private int ticksPerRotation = (int) (DriveGearRatio * 1120);

        private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
        private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
        private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
        private static final String VUFORIA_KEY = "AbZUuPf/////AAAAGUmS0Chan00iu7rnRhzu63+JgDtPo889M6dNtjvv+WKxiMJ8w2DgSJdM2/zEI+a759I7DlPj++D2Ryr5sEHAg4k1bGKdo3BKtkSeh8hCy78w0SIwoOACschF/ImuyP/V259ytjiFtEF6TX4teE8zYpQZiVkCQy0CmHI9Ymoa7NEvFEqfb3S4P6SicguAtQ2NSLJUX+Fdn49SEJKvpSyhwyjbrinJbak7GWqBHcp7fGh7TNFcfPFMacXg28XxlvVpQaVNgkvuqolN7wkTiR9ZMg6Fnm0zN4Xjr5lRtDHeE51Y0bZoBUbyLWSA+ts3SyDjDPPUU7GMI+Ed/ifb0csVpM12aOiNr8d+HsfF2Frnzrj2";
        private VuforiaLocalizer vuforia;
        private TFObjectDetector tfod;
        private String mineralPosition = "";
        private int step = 1;
        private int nextstep = 0;
        private int targetPosition = 0;
        private int position = 1;
        private boolean fl;
        private boolean fr;
        private boolean dl;
        private boolean dr;
        private double gyroTarget;
        private double gyroRange;
        private double minSpeed;
        private double addSpeed;
        private double power;
        private double distance;
        private long startTime;
        private int encoderSign;




        @Override
        public void init() {
            // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
            // first.

            telemetry.addData("Status", "Initialized");
            telemetry.update();

            upperLeftDrive = hardwareMap.get(DcMotor.class, "front left");
            upperRightDrive = hardwareMap.get(DcMotor.class, "front right");
            lowerLeftDrive = hardwareMap.get(DcMotor.class, "back left");
            lowerRightDrive = hardwareMap.get(DcMotor.class, "back right");
            teamMarker = hardwareMap.get(Servo.class, "team marker");
            hook = hardwareMap.get(DcMotor.class, "hook");
            intakeArm = hardwareMap.get(DcMotor.class, "intake arm");
            imu = hardwareMap.get(BNO055IMU.class, "imu");

            //initialization
            upperLeftDrive.setDirection(DcMotor.Direction.FORWARD);
            upperRightDrive.setDirection(DcMotor.Direction.REVERSE);
            lowerLeftDrive.setDirection(DcMotor.Direction.FORWARD);
            lowerRightDrive.setDirection(DcMotor.Direction.REVERSE);

            initVuforia();
            initTfod();
        }

        @Override
        public void init_loop() {
        }

        @Override
        public void start() {
        }

        //if opMode is active

        //assumes there's a bug in the code because there is no communication between the robot and the phones
        //state machine, upon finishing a state, one can go to another state
        //similar to a flow chart, its linear
        //make one variable in case 0, but in each case, make it different things, different ways to refer to the same object
        //do hook and vuforia
        //break makes it exit the switch statement
        //make a variable that is 8400
        @Override
        //you can make a varible hold different objects if you want, for example different motors
        public void loop() {

            switch(step) {
                /*
                case - 2:
                    long currentTime = System.currentTimeMillis();
                    if (((gyroTarget - getGyroRotation(unit) + 360.0) % 360.0) < 2.5 ||
                            currentTime - startTime > 3000) {
                        this.turn(0);
                        step = nextstep;
                    }
                    else {
                        gyroCorrect(gyroTarget, gyroRange, getGyroRotation(unit), minSpeed, addSpeed);
                    }
                    telemetry.addLine(this.getGyroRotation(unit) + "");
                    telemetry.update();

                    break;
*/
                case -1:
                    fl = Math.abs(upperLeftDrive.getCurrentPosition()) >= Math.abs(position) - 20;
                    fr = Math.abs(upperRightDrive.getCurrentPosition()) >= Math.abs(position) - 20;
                    dl = Math.abs(lowerLeftDrive.getCurrentPosition()) >= Math.abs(position) - 20;
                    dr = Math.abs(lowerRightDrive.getCurrentPosition()) >= Math.abs(position) - 20;

                    if (fl && fr && dl && dr) {
                        //update to see if they are still under the target position
                        step = nextstep;

                    }
                    else {
                        upperLeftDrive.setPower(power);
                        upperRightDrive.setPower(power);
                        lowerLeftDrive.setPower(power);
                        lowerRightDrive.setPower(power);
                    }

                    telemetry.addLine(upperLeftDrive.getCurrentPosition() + "");
                    telemetry.addLine(upperRightDrive.getCurrentPosition() + "");
                    telemetry.addLine(lowerLeftDrive.getCurrentPosition() + "");
                    telemetry.addLine(lowerRightDrive.getCurrentPosition() + "");
                    telemetry.update();

                    break;
/*
                case 0:
                    if (hook.getCurrentPosition() >= 8400 - 20) {
                        step = nextstep;
                    }
                    telemetry.addLine("" + hook.getCurrentPosition());
                    telemetry.update();

                    break;
                case 1:
                    DrivePosition(8400);
                    nextstep = 2;
                    step = 0;
                    break;
                case 2:
                    //here, position is 0
                    power = powerScaleDistance(5);
                    encoderSign = this.setDirection(Direction.BACKWARDS);
                    position = DriveTrain(5);
                    nextstep = 3;
                    step = -1;
                    break;
                case 3:
                    gyroTarget = 270.0;
                    gyroRange = 5;
                    minSpeed = .075;
                    addSpeed = .075;
                    upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                    upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                    lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                    lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                    gyroCorrect(270, 5, getGyroRotation(unit), .075, .075);
                    startTime = System.currentTimeMillis();
                    nextstep = 4;
                    step = -2;
                    break;
                    */
                case 1:
                    //this is where the actual tfod recognition takes place
                    //we are doing two mineral sampling, otherwise we have no space
                    if (tfod != null) {
                        tfod.activate();

                        // getUpdatedRecognitions() will return null if no new information is available since
                        // the last time that call was made.
                        List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
                        if (updatedRecognitions != null) {
                            telemetry.addData("# Object Detected", updatedRecognitions.size());
                            if (updatedRecognitions != null) {
                                for (int i = 0; i < updatedRecognitions.size(); i++) {
                                    if (updatedRecognitions.get(i).getWidth() * updatedRecognitions.get(i).getHeight() < 10000 ||
                                            updatedRecognitions.get(i).getWidth() - updatedRecognitions.get(i).getHeight() > 50) {
                                        //If the difference between height and width is larger than 50 it is unlikely to be a mineral
                                        updatedRecognitions.remove(i); // remove the recognition
                                        i--;
                                    }
                                }

                                //there should be two minerals left
                                if (updatedRecognitions.size() == 1) {

                                    //this should simply be one item
                                    for (Recognition recognition : updatedRecognitions) {

                                        //get the label of the recognition according to the neural
                                        //network

                                        //hit the mineral if it is the gold one
                                        if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
                                            mineralPosition = "LEFT";

                                            telemetry.addData("Gold Mineral Position", "Left");
                                            telemetry.update();
                                            power = powerScaleDistance(3);
                                            encoderSign = this.setDirection(Direction.FORWARDS);
                                            position = DriveTrain(3);
                                            nextstep = 7;
                                            step = -1;
                                            break;
                                        }
                                        //otherwise strafe to the other one to the right
                                        else {
                                            power = powerScaleDistance(8);
                                            position = strafeleft(8);
                                            nextstep = 5;
                                            step = -1;
                                            break;
                                        }
                                    }


                                }

                            }
                        }
                    }
                case 5:
                    break;
                case 6:
                    break;
                case 7:
                    break;

            }
            //all other telemetry has been moved up to save time
            telemetry.addLine(step + "");
            telemetry.update();



        /*

        //move the hook
        DrivePosition(8400);
        if (hook.getCurrentPosition() <= 8400 - 20) {
            telemetry.addLine(hook.getCurrentPosition() + "");
            telemetry.update();
        } else if (hook.getCurrentPosition() >= 8400 - 20) {
            hook.setPower(0);
        } else {
            hook.setPower(0);
        }

        //drive off
        DriveTrain(5, "backwards");

        try {
            Thread.sleep(10000);
        }
        catch (Exception e) {
            telemetry.addLine("Error, weeb");
            telemetry.update();
        }

        long startTime = System.currentTimeMillis(); //log when you started to turn

        //point the phone to the minerals
        while (true) {
            gyroCorrect(270, 5, getGyroRotation(unit), .1, .4);

            long currentTime = System.currentTimeMillis(); // get current time
            if (currentTime - startTime > 5000) { // if more than 5 seconds has passed, stop turning and procede
                break; //exit loop
            }
        }

        //if tfod can be initiated
        if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
            initTfod();
        } else {
            telemetry.addData("Sorry!", "This device is not compatible with TFOD");
        }

        telemetry.update();

            // Activate Tensor Flow Object Detection.
            if (tfod != null) {
                tfod.activate();

                //allocate time for tfod to work
                try {
                    Thread.sleep(1000);
                }
                catch (Exception e) {
                    telemetry.addLine("Sorry, error with the code.");
                }


                // getUpdatedRecognitions() will return null if no new information is available since
                // the last time that call was made.
                List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
                if (updatedRecognitions != null) {
                    telemetry.addData("# Object Detected", updatedRecognitions.size());
                    if (updatedRecognitions != null) {
                        for (int i = 0; i < updatedRecognitions.size(); i++) {
                            if (updatedRecognitions.get(i).getWidth() * updatedRecognitions.get(i).getHeight() < 10000 ||
                                    updatedRecognitions.get(i).getWidth() - updatedRecognitions.get(i).getHeight() > 50) {
                                //If the difference between height and width is larger than 50 it is unlikely to be a mineral
                                updatedRecognitions.remove(i); // remove the recognition
                                i--;
                            }
                        }

                        //there should be three minerals left
                        if (updatedRecognitions.size() == 3) {

                            //these initializations are just to check that, later on, the values
                            //are different and thus minerals have been recognized
                            int goldMineralX = -1;
                            int silverMineral1X = -1;
                            int silverMineral2X = -1;

                            //a for each loop to run through each recognition to check for
                            //the gold mineral

                            for (Recognition recognition : updatedRecognitions) {

                                //get the label of the recognition according to the neural
                                //network

                                //save the x coordinate of each identification to subsequent
                                //mineral variable
                                if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
                                    goldMineralX = (int) recognition.getLeft();

                                } else if (silverMineral1X == -1) {
                                    silverMineral1X = (int) recognition.getLeft();

                                } else {
                                    silverMineral2X = (int) recognition.getLeft();
                                }
                            }

                            //if all three minerals were identified
                            if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
                                if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
                                    telemetry.addData("Gold Mineral Position", "Left");
                                    mineralPosition = "left";

                                } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
                                    telemetry.addData("Gold Mineral Position", "Right");
                                    mineralPosition = "right";

                                } else {
                                    telemetry.addData("Gold Mineral Position", "Center");
                                    mineralPosition = "center";

                                }

                            }

                        }

                        //strafe to go closer to the minerals
                        strafeleft(24);

                        //depending on each mineral location,

                        if (mineralPosition.equals("left")) { //if the gold mineral is all the way on the left (from the perspective
                            //of the camera)

                            //go backwards towards the leftmost mineral
                            DriveTrain(30, "backwards");

                            //strafe to hit the mineral
                            strafeleft(12);

                            //strafe back out
                            straferight(12);

                        } else if (mineralPosition.equals("right")) {

                            //go backwards towards the rightmost mineral
                            DriveTrain(30, "forwards");

                            //strafe to hit the mineral
                            strafeleft(12);

                            //strafe back out
                            straferight(12);

                            //go back to the left to get ready for the weird turn
                            DriveTrain(60, "backwards");

                        } else { //most likely, the mineral will be in the center if the above
                            //if statements fail , but in case tfod doesn't
                            //detect any minerals, this gives us a fail-safe

                            //strafe to hit the mineral
                            strafeleft(12);

                            //strafe back out
                            straferight(12);

                            DriveTrain(30, "backwards");
                        }


                        //do the weird turn
                        weirdTurn(24);

                        //strafe closer to hit the wall
                        strafeleft(28);

                        //go towards the depot square
                        DriveTrain(36, "backwards");

                        //turns towards the depot square
                        startTime = System.currentTimeMillis(); //log when you started to turn

                        while (true) {
                            gyroCorrect(-90, 5, getGyroRotation(unit), .1, .4);
                            //correct to the angle of 90
                            long currentTime = System.currentTimeMillis(); // get current time
                            if (currentTime - startTime > 5000) { // if more than 5 seconds has passed (more time than should be needed for a turn), stop turning and procede
                                break; //exit loop
                            }
                        }

                        //deploy the teamMarker
                        teamMarker.setPosition(0);

                        //strafe towards the crater
                        straferight(36);

                        //turn towards the crater
                        startTime = System.currentTimeMillis(); //log when you started to turn

                        //point the phone to the minerals
                        while (true) {
                            gyroCorrect(90, 5, getGyroRotation(unit), .1, .4);
                            //correct to the angle of 90
                            long currentTime = System.currentTimeMillis(); // get current time
                            if (currentTime - startTime > 5000) { // if more than 5 seconds has passed (more time than should be needed for a turn), stop turning and procede
                                break; //exit loop
                            }
                        }

                        //move the arm to the crater
                        DrivePositionArm(-360);
                        telemetry.update();
                    }
                }
            }

            */
        }

        @Override
        public void stop() {
            if (tfod != null) {
                tfod.shutdown();
            }
            telemetry.addLine("Long live the russian regime! (aka Sasha)");
            telemetry.update();
        }

        private void initVuforia() {
            //Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
            //config the vuforia with parameters and variables

            VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();

            parameters.vuforiaLicenseKey = VUFORIA_KEY;
            parameters.cameraDirection = CameraDirection.FRONT;

            //  Instantiate the Vuforia engine
            vuforia = ClassFactory.getInstance().createVuforia(parameters);

            // Loading trackables is not necessary for the Tensor Flow Object Detection engine.
        }

        /**
         * Initialize the Tensor Flow Object Detection engine.
         */
        public void initTfod() {
            //stores an id for where to later draw the image, a window per se
            int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
                    "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());

            TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);

            //implements the parameters, and passes the image to tfod
            tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);

        /* professor Wen states that this is a neural network, so each parameter is linked
        to a specific identification system and we are simply giving names to those final
        identifications to reference them later on */
            tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
        }

        public void DrivePosition(int position) {
            //apparently using negative or positive power here doesn't matter
            hook.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

            hook.setTargetPosition(position);

            hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            hook.setPower(1);
        }

        public void DrivePositionArm(int position) {
            intakeArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

            intakeArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

            intakeArm.setTargetPosition(position);

            intakeArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            intakeArm.setPower(1);
        }

        //for forwards, power = -1, and for backwards, power = 1
        public int DriveTrain(double distance) {
            //example gear ratio, 2:1
            //example circumference
            //distance is in inches

            double circumferenceTraveled = distance / DriveCircumference;

            int position = (int) (ticksPerRotation * circumferenceTraveled);

            position = encoderSign * position;

            double power = powerScaleDistance(distance);

            upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            upperLeftDrive.setTargetPosition(position);
            upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            lowerLeftDrive.setTargetPosition(position);
            lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            upperRightDrive.setTargetPosition(position);
            upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            lowerRightDrive.setTargetPosition(position);
            lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            upperRightDrive.setPower(power);
            lowerLeftDrive.setPower(power);
            upperRightDrive.setPower(power);
            lowerRightDrive.setPower(power);

            return position;
        }

        //power scale according to distance
        public double powerScaleDistance(double distance) {
            double DrivePower = 0;
            if (distance <= 24) {
                DrivePower = 0.5;
            }
            else if (distance > 24) {
                DrivePower = 1;
            }
            return DrivePower;
        }

        public int strafeleft(double distance) {
            //guess and check, then check formulas later on
            //assuming a similar relationship from distance to ticks in the drive train, probably going to
            //be much less

            //how much the wheel actually needs to turn
            double circumferenceTraveled = distance / DriveCircumference;

            //here position must be positive
            int position = (int) (ticksPerRotation * circumferenceTraveled);

            double power = powerScaleDistance(distance);

            upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            upperLeftDrive.setTargetPosition(-position);
            upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            upperLeftDrive.setPower(powerScaleDistance(power));

            upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            upperRightDrive.setTargetPosition(position);
            upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            upperRightDrive.setPower(powerScaleDistance(power));

            lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            lowerLeftDrive.setTargetPosition(position);
            lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            lowerLeftDrive.setPower(powerScaleDistance(power));

            lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            lowerRightDrive.setTargetPosition(-position);
            lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            lowerRightDrive.setPower(powerScaleDistance(power));

            return position;
        }

        public int straferight(double distance) {
            //guess and check, then check formulas later on
            //assuming a similar relationship from distance to ticks in the drive train, probably going to
            //be much less

            double circumferenceTraveled = distance / DriveCircumference;

            int position = (int) (ticksPerRotation * circumferenceTraveled);

            double power = powerScaleDistance(distance);

            upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            upperLeftDrive.setTargetPosition(position);
            upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            upperLeftDrive.setPower(powerScaleDistance(power));

            upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            upperRightDrive.setTargetPosition(-position);
            upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);


            lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            lowerLeftDrive.setTargetPosition(-position);
            lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);


            lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            lowerRightDrive.setTargetPosition(position);
            lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            upperLeftDrive.setPower(powerScaleDistance(power));
            upperRightDrive.setPower(powerScaleDistance(power));
            lowerLeftDrive.setPower(powerScaleDistance(power));
            lowerRightDrive.setPower(powerScaleDistance(power));

            return position;


        }

        public void gyroCorrect(double gyroTarget, double gyroRange, double gyroActual, double minSpeed,
                                double addSpeed) {

            //this makes it an actual angle between 0 and 360 anyways, so it doesn't matter if the target
            //is weird for the gyro or actually between 0 and 360
            double delta = (gyroTarget - gyroActual + 360.0) % 360.0; // in case it is negative

            if (delta > 180.0) {
                delta -= 360.0; // delta becomes between -180 and 180
                //because the range is from 0-> 180 and -180-> 0 instead of 0-> 360
            }

            if (Math.abs(delta) > gyroRange) {
                double gyroMod = delta / 45.0; // if delta is less than 45 and bigger than -45, this will make a scale from
                // -1 to 1

                if (Math.abs(gyroMod) > 1.0) {
                    gyroMod = Math.signum(gyroMod); //makes gyroMod -1 or 1 if error is more than 45
                    // or less than -45 degrees
                }

                //if the error is more than 180, then the power is positive, and it turns to the left
                //if the error is less than 180, the power in the turn in negative, and it turns to the
                //right
                //if the error is larger, faster speed

                this.turn(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod);

                telemetry.addLine(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod + "");
                telemetry.update();

            } else {
                turn(0.0);

            }
        }


        public void turn(double power) { //this is opposite to the varsity because our gear train makes
            //forwards negative and backwards positive
            upperLeftDrive.setPower(power);
            upperRightDrive.setPower(-power);
            lowerLeftDrive.setPower(power);
            lowerRightDrive.setPower(-power);
        }

        //gets current angle position
        public float getGyroRotation(AngleUnit unit) {
            return imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, unit).firstAngle;
        }

        //this is a weird turn because only the left half of the motors move
        public void weirdTurn(double distance) {
            //example gear ratio, 2:1
            //example circumference
            //distance is in inches

            double circumferenceTraveled = distance / DriveCircumference;

            int position = (int) (ticksPerRotation * circumferenceTraveled);

            //if it is forwards or backwards, change sign of position
            double power = powerScaleDistance(distance);

            //position is positive because it will make the motors go backwards
            upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            upperLeftDrive.setTargetPosition(position);
            upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            upperLeftDrive.setPower(powerScaleDistance(power));

            lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            lowerLeftDrive.setTargetPosition(position);
            lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            lowerLeftDrive.setPower(power);
        }

        //depending on the Direction, the position is either positive or negative for the encoders
        public int setDirection(Direction direction) {
            int positionDirection = 0;
            switch (direction) {
                case FORWARDS:
                    positionDirection = 1;
                    break;
                case BACKWARDS:
                    positionDirection = -1;
                    break;
            }
            return positionDirection;
        }
    }