tiltcheck

   public void turnPIDTime(double degrees, double timeoutP, double speed, boolean reset)
    {
        double kp;
        double ti;
        double td;

        if (PID_TURNING_TEMP[0] != 0 || PID_TURNING_TEMP[1] != 0 || PID_TURNING_TEMP[2] != 0)
        {
            kp = PID_TURNING_TEMP[0];
            ti = PID_TURNING_TEMP[1];
            td = PID_TURNING_TEMP[2];
        }
        else
        {
            kp = PID_TURNING[0];
            ti = PID_TURNING[1];
            td = PID_TURNING[2];
        }

        PID pid = new PID(kp,ti,td, -.3,.3);

        robot.setBrakeSettings();
        double error;
        double power;
        double currentTime;
        double prevTime;
        double dt;

        double desiredAng = degrees + getHeading();

        Hawks_Clock runtime = new Hawks_Clock();
        runtime.setRemoveTime(System.nanoTime());
        currentTime = runtime.getTimeSec();
        prevTime = currentTime;
        double timeOut = runtime.getTimeSec() + timeoutP;
        do {
            currentTime = runtime.getTimeSec();
            dt = currentTime - prevTime;
            power = pid.update(desiredAng,getHeading(), dt);
            error = desiredAng - getHeading();
            robot.setDrivePow(-power,power);
            prevTime = currentTime;

            telemetry.addData("heading: ", getHeading());
            telemetry.addData("error: ", error);
            telemetry.update();

        } while (opModeIsActive() && timeOut > runtime.getTimeSec() && Math.abs(error) > 2);
        robot.setDrivePow(0,0);
        telemetry.addData("final heading: ", getHeading());
        if (reset)
        {
            robot.initIMU();
        }
    }
 public enum TiltDir
    {
        FORWARD("FORWARD_TILT"), BACKWARD("BACKWARD_TILT"), NOT("NO_TILT");
        public final String value;
        TiltDir (String value){this.value = value;}
    }
 public TiltDir tiltCheck()
    {
        TiltDir tiltDir = TiltDir.NOT;
        if (!pitchBoolStatus)
        {
            pitchInit = getPitchDeg();
            pitchBoolStatus = true;
        }
        else
        {
            double amountOver = getPitchDeg() + pitchInit;
            if (amountOver > 15)
            {
                //adjust this eventually.
                setDrivePow(.5,.5);
            }
            else if (amountOver < 15)
            {
                //adjust this eventually.
                setDrivePow(-.5,-.5);
            }
        }
        return tiltDir;
    }