DRIVE

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;



    @Autonomous(name="Basic: Linear OpMode", group="Linear Opmode")
    public class DRIVE extends LinearOpMode {

        private DcMotor leftFrontDrive = null;
        private DcMotor rightFrontDrive = null;
        private DcMotor leftBackDrive = null;
        private DcMotor rightBackDrive = null;
        private final double Perimeter = 12.56;
        private final double RotationP = 43.806;
        private final double BACKDRIVEMUL = 0.86;
        private final double LFRONTDRIVEMUL = 0.9;
        private final double deviation = 1.6444444;

        @Override
        public void runOpMode() {

            leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
            leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
            rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
            rightBackDrive.setDirection(DcMotor.Direction.FORWARD);

            rightBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            leftBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

            rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

            leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive");
            leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive");
            rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive");
            rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive");

        }
        public void driveForward(int distance_inch, double speed) {

            int distance_degrees = (int) (1120 * (distance_inch + 5) / Perimeter);

            rightFrontDrive.setTargetPosition(distance_degrees);
            leftFrontDrive.setTargetPosition(distance_degrees);
            rightBackDrive.setTargetPosition(distance_degrees);
            leftBackDrive.setTargetPosition(distance_degrees);

            rightFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            leftFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            rightBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            leftBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            rightBackDrive.setPower(0);
            leftBackDrive.setPower(0);
            rightFrontDrive.setPower(0);
            leftFrontDrive.setPower(0);
            telemetry.addData("Status", "out of busy");
            telemetry.update();
            beginF();
        }
        public void turnDegrees(int degrees, double speed) {

            int turn_degrees = (int) (1120 * ((RotationP / 360 * degrees) / Perimeter) * deviation);

            rightFrontDrive.setTargetPosition(turn_degrees);
            leftFrontDrive.setTargetPosition(-turn_degrees);
            rightBackDrive.setTargetPosition(turn_degrees);
            leftBackDrive.setTargetPosition(-turn_degrees);

            rightFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            leftFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            rightBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            leftBackDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            rightFrontDrive.setPower(speed);
            leftFrontDrive.setPower(-(LFRONTDRIVEMUL * speed));
            rightBackDrive.setPower(BACKDRIVEMUL * speed);
            leftBackDrive.setPower(-(BACKDRIVEMUL * speed));

            rightBackDrive.setPower(0);
            leftBackDrive.setPower(0);
            rightFrontDrive.setPower(0);
            leftFrontDrive.setPower(0);
            beginF();
        }
        public void beginF(){

            rightBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            leftBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

            rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        }
    }