TeamCode/src/main/java/org/firstinspires/ftc/teamcode/encoderAuto.java
public void encoderDrive(double speed,
                             double leftInches, double rightInches,
                             double timeoutS) {
        int newLeftTarget;
        int newRightTarget;

        // Ensure that the opmode is still active
        if (opModeIsActive()) {

            // Determine new target position, and pass to motor controller
            newLeftTarget = bsgbot.frontLeft.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
            newRightTarget = bsgbot.frontRight.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
            newLeftTarget = bsgbot.backLeft.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
            newRightTarget = bsgbot.backRight.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);

            bsgbot.frontLeft.setTargetPosition(newLeftTarget);
            bsgbot.frontRight.setTargetPosition(newRightTarget);
            bsgbot.backLeft.setTargetPosition(newLeftTarget);
            bsgbot.backRight.setTargetPosition(newRightTarget);

            // Turn On RUN_TO_POSITION
            bsgbot.frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            bsgbot.frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            bsgbot.backLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            bsgbot.backRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            // reset the timeout time and start motion.
            runtime.reset();
            bsgbot.frontLeft.setPower(Math.abs(speed));
            bsgbot.frontRight.setPower(Math.abs(speed));
            bsgbot.backLeft.setPower(Math.abs(speed));
            bsgbot.backRight.setPower(Math.abs(speed));

            // keep looping while we are still active, and there is time left, and both motors are running.
            // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
            // its target position, the motion will stop.  This is "safer" in the event that the robot will
            // always end the motion as soon as possible.
            // However, if you require that BOTH motors have finished their moves before the robot continues
            // onto the next step, use (isBusy() || isBusy()) in the loop test.
            while (opModeIsActive() &&
                    (runtime.seconds() < timeoutS) &&
                    (bsgbot.frontLeft.isBusy() && bsgbot.frontRight.isBusy() && bsgbot.backLeft.isBusy() && bsgbot.backRight.isBusy())) {

                // Display it for the driver.
                telemetry.addData("Path1",  "Running to %7d :%7d", newLeftTarget,  newRightTarget);
                telemetry.addData("Path2",  "Running at %7d :%7d",
                        bsgbot.frontLeft.getCurrentPosition(),
                        bsgbot.frontRight.getCurrentPosition(),
                        bsgbot.backLeft.getCurrentPosition(),
                        bsgbot.backRight.getCurrentPosition());
                telemetry.update();
            }

            // Stop all motion;
            bsgbot.frontLeft.setPower(0);
            bsgbot.frontRight.setPower(0);
            bsgbot.backLeft.setPower(0);
            bsgbot.backRight.setPower(0);

            // Turn off RUN_TO_POSITION
            bsgbot.frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            bsgbot.frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            bsgbot.backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            bsgbot.backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            

            //  sleep(250);   // optional pause after each move
        }
    }
/*
     *  Method to perform a relative move, based on encoder counts.
     *  Encoders are not reset as the move is based on the current position.
     *  Move will stop if any of three conditions occur:
     *  1) Move gets to the desired position
     *  2) Move runs out of time
     *  3) Driver stops the opmode running.
     */
Method to perform a relative move, based on encoder counts.
Encoders are not reset as the move is based on the current position.
Move will stop if any of three conditions occur:
1) Move gets to the desired position
2) Move runs out of time
3) Driver stops the opmode running.
Method to perform a relative move based on encoder counts . Encoders are not reset as the move is based on the current position . Move will stop if any of three conditions occur : 1 ) Move gets to the desired position 2 ) Move runs out of time 3 ) Driver stops the opmode running .
public void encoderDrive(double speed,
                             double leftInches, double rightInches,
                             double timeoutS) {
        int newLeftTarget;
        int newRightTarget;
        if (opModeIsActive()) {
            newLeftTarget = bsgbot.frontLeft.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
            newRightTarget = bsgbot.frontRight.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
            newLeftTarget = bsgbot.backLeft.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
            newRightTarget = bsgbot.backRight.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
            bsgbot.frontLeft.setTargetPosition(newLeftTarget);
            bsgbot.frontRight.setTargetPosition(newRightTarget);
            bsgbot.backLeft.setTargetPosition(newLeftTarget);
            bsgbot.backRight.setTargetPosition(newRightTarget);
            bsgbot.frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            bsgbot.frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            bsgbot.backLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            bsgbot.backRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            runtime.reset();
            bsgbot.frontLeft.setPower(Math.abs(speed));
            bsgbot.frontRight.setPower(Math.abs(speed));
            bsgbot.backLeft.setPower(Math.abs(speed));
            bsgbot.backRight.setPower(Math.abs(speed));
            while (opModeIsActive() &&
                    (runtime.seconds() < timeoutS) &&
                    (bsgbot.frontLeft.isBusy() && bsgbot.frontRight.isBusy() && bsgbot.backLeft.isBusy() && bsgbot.backRight.isBusy())) {
                telemetry.addData("Path1",  "Running to %7d :%7d", newLeftTarget,  newRightTarget);
                telemetry.addData("Path2",  "Running at %7d :%7d",
                        bsgbot.frontLeft.getCurrentPosition(),
                        bsgbot.frontRight.getCurrentPosition(),
                        bsgbot.backLeft.getCurrentPosition(),
                        bsgbot.backRight.getCurrentPosition());
                telemetry.update();
            }
            bsgbot.frontLeft.setPower(0);
            bsgbot.frontRight.setPower(0);
            bsgbot.backLeft.setPower(0);
            bsgbot.backRight.setPower(0);
            bsgbot.frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            bsgbot.frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            bsgbot.backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            bsgbot.backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        }
    }
public void encoderDrive ( double speed , double leftInches , double rightInches , double timeoutS ) { int newLeftTarget ; int newRightTarget ; if ( opModeIsActive ( ) ) { newLeftTarget = bsgbot . frontLeft . getCurrentPosition ( ) + ( int ) ( leftInches * COUNTS_PER_INCH ) ; newRightTarget = bsgbot . frontRight . getCurrentPosition ( ) + ( int ) ( rightInches * COUNTS_PER_INCH ) ; newLeftTarget = bsgbot . backLeft . getCurrentPosition ( ) + ( int ) ( leftInches * COUNTS_PER_INCH ) ; newRightTarget = bsgbot . backRight . getCurrentPosition ( ) + ( int ) ( rightInches * COUNTS_PER_INCH ) ; bsgbot . frontLeft . setTargetPosition ( newLeftTarget ) ; bsgbot . frontRight . setTargetPosition ( newRightTarget ) ; bsgbot . backLeft . setTargetPosition ( newLeftTarget ) ; bsgbot . backRight . setTargetPosition ( newRightTarget ) ; bsgbot . frontLeft . setMode ( DcMotor . RunMode . RUN_TO_POSITION ) ; bsgbot . frontRight . setMode ( DcMotor . RunMode . RUN_TO_POSITION ) ; bsgbot . backLeft . setMode ( DcMotor . RunMode . RUN_TO_POSITION ) ; bsgbot . backRight . setMode ( DcMotor . RunMode . RUN_TO_POSITION ) ; runtime . reset ( ) ; bsgbot . frontLeft . setPower ( Math . abs ( speed ) ) ; bsgbot . frontRight . setPower ( Math . abs ( speed ) ) ; bsgbot . backLeft . setPower ( Math . abs ( speed ) ) ; bsgbot . backRight . setPower ( Math . abs ( speed ) ) ; while ( opModeIsActive ( ) && ( runtime . seconds ( ) < timeoutS ) && ( bsgbot . frontLeft . isBusy ( ) && bsgbot . frontRight . isBusy ( ) && bsgbot . backLeft . isBusy ( ) && bsgbot . backRight . isBusy ( ) ) ) { telemetry . addData ( "Path1" , "Running to %7d :%7d" , newLeftTarget , newRightTarget ) ; telemetry . addData ( "Path2" , "Running at %7d :%7d" , bsgbot . frontLeft . getCurrentPosition ( ) , bsgbot . frontRight . getCurrentPosition ( ) , bsgbot . backLeft . getCurrentPosition ( ) , bsgbot . backRight . getCurrentPosition ( ) ) ; telemetry . update ( ) ; } bsgbot . frontLeft . setPower ( 0 ) ; bsgbot . frontRight . setPower ( 0 ) ; bsgbot . backLeft . setPower ( 0 ) ; bsgbot . backRight . setPower ( 0 ) ; bsgbot . frontLeft . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; bsgbot . frontRight . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; bsgbot . backLeft . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; bsgbot . backRight . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; } }
Method to perform a relative move, based on encoder counts.
Method to perform a relative move based on encoder counts .
// Ensure that the opmode is still active // Determine new target position, and pass to motor controller // Turn On RUN_TO_POSITION // reset the timeout time and start motion. // keep looping while we are still active, and there is time left, and both motors are running. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits // its target position, the motion will stop.  This is "safer" in the event that the robot will // always end the motion as soon as possible. // However, if you require that BOTH motors have finished their moves before the robot continues // onto the next step, use (isBusy() || isBusy()) in the loop test. // Display it for the driver. // Stop all motion; // Turn off RUN_TO_POSITION //  sleep(250);   // optional pause after each move