encoderturn11.2_needToWork

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;


@Autonomous(name = "test_turn")
public class encoder_turn extends LinearOpMode
{
    private HardWare11226 hardware = new HardWare11226();
    boolean runonce = true;

    @Override
    public void runOpMode() throws InterruptedException
    {

        hardware.init(hardwareMap);
        telemetry.addData("Status", "init");


        hardware.leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        hardware.rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        hardware.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        hardware.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        waitForStart();

        hardware.leftMotor.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
        hardware.rightMotor.setDirection(DcMotor.Direction.REVERSE);

        while (opModeIsActive())
        {
            if (runonce == true)
            {
                turning(90, -0.4, 0.4);
                runonce = false;
            }
        }
    }

    public void turning(double angle, double right_power, double left_power)
    {
        hardware.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        hardware.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        int leftTarget = (int) (((120.89 / 31.9024 ) / 360) * angle);
        int rightTarget = (int) (((120.89 / 31.9024 ) / 360) * angle);

        hardware.leftMotor.setTargetPosition(leftTarget);
        hardware.rightMotor.setTargetPosition(-rightTarget);

        hardware.leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        hardware.rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        hardware.leftMotor.setPower(left_power);
        hardware.rightMotor.setPower(right_power);

        while (hardware.rightMotor.isBusy())
        {
            telemetry.addData("Status: ", "isBusy.");
            telemetry.addData("left pos", hardware.leftMotor.getCurrentPosition());
            telemetry.addData("right pos", hardware.rightMotor.getCurrentPosition());
            telemetry.update();
        }

        hardware.leftMotor.setPower(0);
        hardware.rightMotor.setPower(0);

        telemetry.update();

        hardware.leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        hardware.rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    }
}