TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonOpMode.java
public void rotate(int degrees, double power) {
        // restart imu angle tracking.
        resetAngle();

        // if degrees > 359 we cap at 359 with same sign as original degrees.
        if (Math.abs(degrees) > 359) degrees = (int) Math.copySign(359, degrees);

        // start pid controller. PID controller will monitor the turn angle with respect to the
        // target angle and reduce power as we approach the target angle. This is to prevent the
        // robots momentum from overshooting the turn after we turn off the power. The PID controller
        // reports onTarget() = true when the difference between turn angle and target angle is within
        // 1% of target (tolerance) which is about 1 degree. This helps prevent overshoot. Overshoot is
        // dependant on the motor and gearing configuration, starting power, weight of the robot and the
        // on target tolerance. If the controller overshoots, it will reverse the sign of the output
        // turning the robot back toward the setpoint value.

        pidRotate.reset();
        pidRotate.setSetpoint(degrees);
        pidRotate.setInputRange(0, degrees);
        pidRotate.setOutputRange(0, power);
        pidRotate.setTolerance(1);
        pidRotate.enable();

        // getAngle() returns + when rotating counter clockwise (left) and - when rotating
        // clockwise (right).

        // rotate until turn is completed.

        if (degrees < 0) {
            do { // right turn
                power = pidRotate.performPID(getAngle()); // power will be - on right turn.
                backLeftMotor.setPower(power);
                frontLeftMotor.setPower(power);
                backRightMotor.setPower(power);
                frontRightMotor.setPower(power);
            } while (opModeIsActive() && getAngle() > degrees);
        } else
            do { // left turn
                power = pidRotate.performPID(getAngle()); // power will be + on left turn.
                backLeftMotor.setPower(power);
                frontLeftMotor.setPower(power);
                backRightMotor.setPower(power);
                frontRightMotor.setPower(power);
            } while (opModeIsActive() && getAngle() < degrees);

        // turn the motors off.
        stopDriveMotors();

        // wait for rotation to stop.
        sleep(250);

        // reset angle tracking on new heading.
        resetAngle();
    }
/**
     * Rotate left or right the number of degrees. Does not support turning more than 180 degrees.
     *
     * @param degrees Degrees to turn, + is left - is right
     */
Rotate left or right the number of degrees. Does not support turning more than 180 degrees.
@param degrees Degrees to turn, + is left - is right
Rotate left or right the number of degrees . Does not support turning more than 180 degrees . @param degrees Degrees to turn + is left - is right
public void rotate(int degrees, double power) {
        resetAngle();
        if (Math.abs(degrees) > 359) degrees = (int) Math.copySign(359, degrees);
        pidRotate.reset();
        pidRotate.setSetpoint(degrees);
        pidRotate.setInputRange(0, degrees);
        pidRotate.setOutputRange(0, power);
        pidRotate.setTolerance(1);
        pidRotate.enable();
        if (degrees < 0) {
            do { 
                power = pidRotate.performPID(getAngle()); 
                backLeftMotor.setPower(power);
                frontLeftMotor.setPower(power);
                backRightMotor.setPower(power);
                frontRightMotor.setPower(power);
            } while (opModeIsActive() && getAngle() > degrees);
        } else
            do { 
                power = pidRotate.performPID(getAngle()); 
                backLeftMotor.setPower(power);
                frontLeftMotor.setPower(power);
                backRightMotor.setPower(power);
                frontRightMotor.setPower(power);
            } while (opModeIsActive() && getAngle() < degrees);
        stopDriveMotors();
        sleep(250);
        resetAngle();
    }
public void rotate ( int degrees , double power ) { resetAngle ( ) ; if ( Math . abs ( degrees ) > 359 ) degrees = ( int ) Math . copySign ( 359 , degrees ) ; pidRotate . reset ( ) ; pidRotate . setSetpoint ( degrees ) ; pidRotate . setInputRange ( 0 , degrees ) ; pidRotate . setOutputRange ( 0 , power ) ; pidRotate . setTolerance ( 1 ) ; pidRotate . enable ( ) ; if ( degrees < 0 ) { do { power = pidRotate . performPID ( getAngle ( ) ) ; backLeftMotor . setPower ( power ) ; frontLeftMotor . setPower ( power ) ; backRightMotor . setPower ( power ) ; frontRightMotor . setPower ( power ) ; } while ( opModeIsActive ( ) && getAngle ( ) > degrees ) ; } else do { power = pidRotate . performPID ( getAngle ( ) ) ; backLeftMotor . setPower ( power ) ; frontLeftMotor . setPower ( power ) ; backRightMotor . setPower ( power ) ; frontRightMotor . setPower ( power ) ; } while ( opModeIsActive ( ) && getAngle ( ) < degrees ) ; stopDriveMotors ( ) ; sleep ( 250 ) ; resetAngle ( ) ; }
Rotate left or right the number of degrees.
Rotate left or right the number of degrees .
// restart imu angle tracking. // if degrees > 359 we cap at 359 with same sign as original degrees. // start pid controller. PID controller will monitor the turn angle with respect to the // target angle and reduce power as we approach the target angle. This is to prevent the // robots momentum from overshooting the turn after we turn off the power. The PID controller // reports onTarget() = true when the difference between turn angle and target angle is within // 1% of target (tolerance) which is about 1 degree. This helps prevent overshoot. Overshoot is // dependant on the motor and gearing configuration, starting power, weight of the robot and the // on target tolerance. If the controller overshoots, it will reverse the sign of the output // turning the robot back toward the setpoint value. // getAngle() returns + when rotating counter clockwise (left) and - when rotating // clockwise (right). // rotate until turn is completed. // right turn // power will be - on right turn. // left turn // power will be + on left turn. // turn the motors off. // wait for rotation to stop. // reset angle tracking on new heading.