package com.qualcomm.ftcrobotcontroller.opmodes.customCode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorController;

public class Autonomous extends LinearOpMode {
    private DcMotor right;
    private DcMotor left;

    final static int ENCODER_CPR = 1440;    //encoder counts per revolution
    final static double GEAR_RATIO = 1;     //gear ratio
    final static double WHEEL_DIAMETER = 2.625;     //diameter of wheel
    final static double CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER;
    final static int DISTANCE = 5;
    final static double ROTATIONS = DISTANCE / CIRCUMFERENCE;
    final static double COUNTS = ENCODER_CPR * ROTATIONS * GEAR_RATIO;
    final static int ERROR_THRESHOLD = 10;

    @Override
    public void runOpMode() throws InterruptedException {
        right = hardwareMap.dcMotor.get("Right");
        left = hardwareMap.dcMotor.get("Left");
        right.setDirection(DcMotor.Direction.REVERSE);

        right.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
        left.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
        telemetry.addData("Reset Encoders", "Done");
        //distance = 12;
        right.setTargetPosition((int) COUNTS);
        left.setTargetPosition((int) COUNTS);

        right.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
        left.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
        telemetry.addData("Running to Target", "Started");

        telemetry.addData("Motor Target", COUNTS);
        telemetry.addData("Left Position", left.getCurrentPosition());
        telemetry.addData("Right Position", right.getCurrentPosition());

        right.setPower(0.5);
        left.setPower(0.5);
        telemetry.addData("Setting Power", "Set");

        telemetry.addData("Motor Target", COUNTS);
        telemetry.addData("Left Position", left.getCurrentPosition());
        telemetry.addData("Right Position", right.getCurrentPosition());

        while (true) { // && left.getCurrentPosition() == DISTANCE) {
            telemetry.addData("Motor Target", COUNTS);
            telemetry.addData("Left Position", left.getCurrentPosition());
            telemetry.addData("Right Position", right.getCurrentPosition());
            int error = Math.abs(right.getCurrentPosition()) - (int) COUNTS;

            if (error < ERROR_THRESHOLD) {
                right.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
                left.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
                telemetry.addData("Final Reset", "Done");
                break;
            }
            //waitOneHardwareCycle();
        }

    }
}

@@ -814,43 +814,28 @@
     
-@Override%0A    public void runOpMode
+private void etSetup
 () t
@@ -863,18 +863,16 @@
 ption %7B%0A
-  
       ri
@@ -913,26 +913,24 @@
 ht%22);%0A      
- 
- 
 left = hardw
@@ -953,26 +953,24 @@
 et(%22Left%22);%0A
-  
       right.
@@ -1009,34 +1009,32 @@
 EVERSE);%0A%0A      
- 
- 
 right.setChannel
@@ -1077,26 +1077,24 @@
 _ENCODERS);%0A
-  
       left.s
@@ -1148,34 +1148,32 @@
 NCODERS);%0A      
-  
 telemetry.addDat
@@ -1197,26 +1197,24 @@
 %22, %22Done%22);%0A
-  
       //dist
@@ -1226,26 +1226,24 @@
 = 12;%0A      
-  
 right.setTar
@@ -1267,34 +1267,32 @@
  COUNTS);%0A      
- 
- 
 left.setTargetPo
@@ -1310,26 +1310,24 @@
 ) COUNTS);%0A%0A
-  
       right.
@@ -1383,34 +1383,32 @@
 OSITION);%0A      
- 
- 
 left.setChannelM
@@ -1451,26 +1451,24 @@
 _POSITION);%0A
-  
       teleme
@@ -1511,34 +1511,32 @@
 arted%22);%0A%0A      
-  
 telemetry.addDat
@@ -1554,34 +1554,32 @@
 rget%22, COUNTS);%0A
-  
       telemetry.
@@ -1629,34 +1629,32 @@
 ition());%0A      
-  
 telemetry.addDat
@@ -1705,26 +1705,24 @@
 ());%0A%0A      
- 
- 
 right.setPow
@@ -1732,26 +1732,24 @@
 0.5);%0A      
-  
 left.setPowe
@@ -1754,34 +1754,32 @@
 wer(0.5);%0A      
- 
- 
 telemetry.addDat
@@ -1802,26 +1802,24 @@
 %22, %22Set%22);%0A%0A
-  
       teleme
@@ -1853,34 +1853,32 @@
  COUNTS);%0A      
-  
 telemetry.addDat
@@ -1920,26 +1920,24 @@
 osition());%0A
-  
       teleme
@@ -1999,85 +1999,75 @@
 ));%0A
+    %7D
 %0A    
+%0A
     
-while (true) %7B // && left.getCurrentPosition() == DISTANCE) %7B%0A      
+private void etLoop() throws InterruptedException %7B%0A
     
@@ -2107,30 +2107,24 @@
 %22, COUNTS);%0A
-      
       teleme
@@ -2182,30 +2182,24 @@
 n());%0A      
- 
-     
 telemetry.ad
@@ -2257,22 +2257,16 @@
 ;%0A      
-  
-    
 int erro
@@ -2329,22 +2329,16 @@
 %0A%0A      
-   
-   
 if (erro
@@ -2360,22 +2360,16 @@
 HOLD) %7B%0A
-      
         
@@ -2444,22 +2444,16 @@
         
-     
- 
 left.set
@@ -2503,30 +2503,24 @@
 _ENCODERS);%0A
-      
           te
@@ -2569,22 +2569,16 @@
         
-      
 break;%0A 
@@ -2578,26 +2578,58 @@
 reak;%0A      
+%7D%0A
   
+    //waitOneHardwareCycle();%0A
     %7D%0A      
@@ -2630,41 +2630,228 @@
     
+%0A
     
-    //waitOneHardwareCycle();
+@Override%0A    public void runOpMode() throws InterruptedException %7B%0A      %0A        etSetup();%0A        while (opModeIsActive()) %7B // && left.getCurrentPosition() == DISTANCE) %7B%0A        %0A          etLoop();%0A          
 %0A   

Update to OPMIA and new format