PID Control X Drive

/**
     * @param speed    can be any number UNDER 1, the lower the better
     * @param maxThres must be equal to 1 - speed, no more or less
     * @param distance is how far you want to move, in ticks.
     * @param heading  is the wanted heading in degrees
     */

    public void driveByPID(double speed, double maxThres, int distance, double heading) {
        //This grabs the error, and passes it the heading.
        double error = getError(heading);
        //This creates the proportional multiplier.
        double multiplier = (error / 180) * maxThres;

        //I forget if it works exactly like this or not. Guess we'll find you.

        ForwardRight.setTargetPosition(distance);
        BackRight.setTargetPosition(distance);
        ForwardLeft.setTargetPosition(distance);
        BackLeft.setTargetPosition(distance);

        //The theory behind this is that it will turn like a PID would.
        while (ForwardRight.isBusy() && BackRight.isBusy() && ForwardLeft.isBusy() && BackLeft.isBusy()) {
            //This grabs the error, and passes it the heading.
            error = getError(heading);
            //This creates the proportional multiplier.
            multiplier = (error / 180) * maxThres;
            ForwardRight.setPower(speed - multiplier);
            BackRight.setPower(speed - multiplier);
            ForwardLeft.setPower(speed + multiplier);
            BackLeft.setPower(speed + multiplier);
        }

    }

    public double getError(double heading) {
        double error;
        error = heading - imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
        return error;
    }