PID Thing

PIDController turnController = new PIDController(pidCoefficients[0][0], pidCoefficients[0][1], pidCoefficients[0][2], -1, 1);

while (Math.abs(imu.getHeading() - targetHeading) > tolerance
      && op.opModeIsActive()) {
    double currentHeading = imu.getHeading();
    double out = turnController.output(currentHeading, targetHeading, op.time);
    setPower(leftMotors, -out);
    setPower(rightMotors, out);
}


public double output(double current, double target, double time) {
        this.input = current;
        this.setpoint = target;

        error = setpoint - input;

        double di = input - lastInput;
        double dt = time - lastTime;

        if ((Math.abs(intError + (error * dt)) * ki < maxOut)
                && (Math.abs(intError + (error * dt)) * ki > minOut))
            intError += (error * dt);

        double p = kp * error;
        double i = ki * intError;
        double d = kd * di;

        double u = p + i + d;

        if(u > maxOut) u = maxOut;
        else if(u < minOut) u = minOut;

        lastInput = input;
        lastTime = time;
        return u;
    }