ACM: Got the sign wrong. I'm using the derivative of the error now and not the sensor, so the sign was reversed.