Call the output(float ek) with the error that you just calculated this will give you the output to apply. Call this code with a fixed interval/sample rate. The constructor takes four arguments P, I, D and the sample rate. You will need to do some tuning on the P, I and D to get the desired result. I will move the last part of this discussion to a new thread when I have added a working example to the firmware.