TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomus/Autonomus_Blue_Foundation_Wall.java
/*
     *  Method to perfmorm 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 perfmorm 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 perfmorm 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 leftfrontInches, double rightfrontInches, double leftrearInches, double rightrearInches,
                             double timeoutS) {
        int newLeftfrontTarget;
        int newRightfronTarget;
        int newlefttrearTarget;
        int newRightrearTarget;
        if (opModeIsActive()) {
            newLeftfrontTarget = robot.leftfrontDrive.getCurrentPosition() + (int)(leftfrontInches * COUNTS_PER_INCH);
            newlefttrearTarget = robot.leftrearDrive.getCurrentPosition() + (int)(leftrearInches * COUNTS_PER_INCH);
            newRightfronTarget = robot.rightfrontDrive.getCurrentPosition() + (int)(rightfrontInches * COUNTS_PER_INCH);
            newRightrearTarget = robot.rightrearDrive.getCurrentPosition() + (int)(rightrearInches * COUNTS_PER_INCH);
            robot.leftfrontDrive.setTargetPosition(newLeftfrontTarget);
            robot.leftrearDrive.setTargetPosition(newlefttrearTarget);
            robot.rightfrontDrive.setTargetPosition(newRightfronTarget);
            robot.rightrearDrive.setTargetPosition(newRightrearTarget);
            robot.leftfrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            robot.leftrearDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            robot.rightfrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            robot.rightrearDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            runtime.reset();
            robot.leftfrontDrive.setPower(Math.abs(speed));
            robot.leftrearDrive.setPower(Math.abs(speed));
            robot.rightfrontDrive.setPower(Math.abs(speed));
            robot.rightrearDrive.setPower(Math.abs(speed));
            while (opModeIsActive() &&
                   (runtime.seconds() < timeoutS) &&
                   (robot.leftfrontDrive.isBusy() && robot.rightfrontDrive.isBusy() &&
            robot.leftrearDrive.isBusy() && robot.rightrearDrive.isBusy()
                   )) {
                telemetry.addData("Path1",  "Running to %7d :%7d", newLeftfrontTarget,  newRightfronTarget);
                telemetry.addData("Path1",  "Running to %7d :%7d", newlefttrearTarget,  newRightrearTarget);
                telemetry.addData("Path2",  "Running at %7d :%7d", robot.leftfrontDrive.getCurrentPosition(), robot.leftrearDrive.getCurrentPosition());
                telemetry.addData("Path2",  "Running at %7d :%7d", robot.rightfrontDrive.getCurrentPosition(), robot.rightrearDrive.getCurrentPosition());
                telemetry.update();
            }
            robot.leftrearDrive.setPower(0);
            robot.leftfrontDrive.setPower(0);
            robot.rightfrontDrive.setPower(0);
            robot.rightrearDrive.setPower(0);
            /* Turn off RUN_TO_POSITION */
            robot.leftfrontDrive.setMode(RUN_USING_ENCODER);
            robot.leftrearDrive.setMode(RUN_USING_ENCODER);
            robot.rightfrontDrive.setMode(RUN_USING_ENCODER);
            robot.rightrearDrive.setMode(RUN_USING_ENCODER);
             sleep(1000);   
        }
    }
public void encoderDrive ( double speed , double leftfrontInches , double rightfrontInches , double leftrearInches , double rightrearInches , double timeoutS ) { int newLeftfrontTarget ; int newRightfronTarget ; int newlefttrearTarget ; int newRightrearTarget ; if ( opModeIsActive ( ) ) { newLeftfrontTarget = robot . leftfrontDrive . getCurrentPosition ( ) + ( int ) ( leftfrontInches * COUNTS_PER_INCH ) ; newlefttrearTarget = robot . leftrearDrive . getCurrentPosition ( ) + ( int ) ( leftrearInches * COUNTS_PER_INCH ) ; newRightfronTarget = robot . rightfrontDrive . getCurrentPosition ( ) + ( int ) ( rightfrontInches * COUNTS_PER_INCH ) ; newRightrearTarget = robot . rightrearDrive . getCurrentPosition ( ) + ( int ) ( rightrearInches * COUNTS_PER_INCH ) ; robot . leftfrontDrive . setTargetPosition ( newLeftfrontTarget ) ; robot . leftrearDrive . setTargetPosition ( newlefttrearTarget ) ; robot . rightfrontDrive . setTargetPosition ( newRightfronTarget ) ; robot . rightrearDrive . setTargetPosition ( newRightrearTarget ) ; robot . leftfrontDrive . setMode ( DcMotor . RunMode . RUN_TO_POSITION ) ; robot . leftrearDrive . setMode ( DcMotor . RunMode . RUN_TO_POSITION ) ; robot . rightfrontDrive . setMode ( DcMotor . RunMode . RUN_TO_POSITION ) ; robot . rightrearDrive . setMode ( DcMotor . RunMode . RUN_TO_POSITION ) ; runtime . reset ( ) ; robot . leftfrontDrive . setPower ( Math . abs ( speed ) ) ; robot . leftrearDrive . setPower ( Math . abs ( speed ) ) ; robot . rightfrontDrive . setPower ( Math . abs ( speed ) ) ; robot . rightrearDrive . setPower ( Math . abs ( speed ) ) ; while ( opModeIsActive ( ) && ( runtime . seconds ( ) < timeoutS ) && ( robot . leftfrontDrive . isBusy ( ) && robot . rightfrontDrive . isBusy ( ) && robot . leftrearDrive . isBusy ( ) && robot . rightrearDrive . isBusy ( ) ) ) { telemetry . addData ( "Path1" , "Running to %7d :%7d" , newLeftfrontTarget , newRightfronTarget ) ; telemetry . addData ( "Path1" , "Running to %7d :%7d" , newlefttrearTarget , newRightrearTarget ) ; telemetry . addData ( "Path2" , "Running at %7d :%7d" , robot . leftfrontDrive . getCurrentPosition ( ) , robot . leftrearDrive . getCurrentPosition ( ) ) ; telemetry . addData ( "Path2" , "Running at %7d :%7d" , robot . rightfrontDrive . getCurrentPosition ( ) , robot . rightrearDrive . getCurrentPosition ( ) ) ; telemetry . update ( ) ; } robot . leftrearDrive . setPower ( 0 ) ; robot . leftfrontDrive . setPower ( 0 ) ; robot . rightfrontDrive . setPower ( 0 ) ; robot . rightrearDrive . setPower ( 0 ) ; /* Turn off RUN_TO_POSITION */ robot . leftfrontDrive . setMode ( RUN_USING_ENCODER ) ; robot . leftrearDrive . setMode ( RUN_USING_ENCODER ) ; robot . rightfrontDrive . setMode ( RUN_USING_ENCODER ) ; robot . rightrearDrive . setMode ( RUN_USING_ENCODER ) ; sleep ( 1000 ) ; } }
Method to perfmorm a relative move, based on encoder counts.
Method to perfmorm a relative move based on encoder counts .
// Ensure that the opmode is still active // Determine new target position, and pass to motor controller //newLeftfrontTarget =  (int)(leftInches * COUNTS_PER_INCH); //newlefttrearTarget = (int)(leftInches * COUNTS_PER_INCH); // newRightfronTarget =   (int)(rightInches * COUNTS_PER_INCH); //newRightrearTarget =   (int)(rightInches * COUNTS_PER_INCH); // 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; // optional pause after each move