public float calculate(float input, float setpoint) {

		if (prevTime != -1) {

			float dt = (System.nanoTime() - prevTime) / 1000000000.0f;
			prevTime = System.nanoTime();

			float err = setpoint - input;
			float diff = (err - prevErr) / dt;
			integration += err * dt;

			prevErr = err;

			if (integration > imax)
				integration = imax;
			if (integration < -imax)
				integration = -imax;

			return (err * p + integration * i + diff * d) * direction;

		} else {
			prevTime = System.nanoTime();
			return 0;
		}

	}