קמבםגקר

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 = "cm")
public class encoder 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)
            {
                driving(40, 0.4);
                runonce = false;
            }
        }
    }

    public void driving(double distance, double power)
    {
        hardware.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        hardware.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        int Target = (int) ((distance / 31.9024) * 1120);

        hardware.leftMotor.setTargetPosition(Target);
        hardware.rightMotor.setTargetPosition(Target);

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

        hardware.leftMotor.setPower(power);
        hardware.rightMotor.setPower(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);
    }
}