pid closed loop feedback control

(Beginning is extra information, my question related to the PID part)

Dear forum member,

I am designing a closed loop system using SigmaDSP and external hardware. Basically, we take the input signal from the photodetector, demodulate(multiply with reference signal) and pass it through LPF and obtain an in-phase signal(also mentioned as an error signal). 

There are coils and we apply a signal with constant amplitude and changing frequency. The in-phase signals amplitude changes(between 1 and -1) with respect to the frequency applied to the coils. We have to apply the frequency which will make the in-phase(error) signal zero. We have to constantly track the error signal and change the frequency if the error signal becomes nonzero. Basically, at 8500Hz, the error signal is +1 and decreases linearly. At 8610Hz it becomes 0V and at 8720Hz the error signal becomes -1V. My purpose is to adjust the frequency in a way to keep the error signal at 0V. (add freq on a positive error and subtract on negative)

First, the approximate resonance frequency(is a frequency which will make the error signal close to zero) is found, then the PID closed loop is applied to fine-tune the error signal and make the error signal exactly zero. With changing the magnetic field, changes the resonance frequency, so the PID must track the error signal and change the frequency accordingly. 

I have designed a PID system, but the output(error signal) never becoming stable. It keeps oscillating around zero. I presume the problem might be in the derivative part. 

The code is as follows, I tried to design a similar system in Sigma Studio.

/*The proportional value of the PID is just a proportional constant
multiplied by the error*/

pid_p = kp*error;

/*To integrate we just sum the previous integral value with the
error multiplied by the integral constant. This will integrate (increase)
the value each loop till we reach the 0 point*/

pid_i = pid_i+(ki*error);

/*The last part is the derivate. The derivate acts upon the speed of the error.
As we know the speed is the amount of error produced in a certain amount of
time divided by that time. For that, we will use a variable called previous_error.
We subtract that value from the actual error and divide all by the elapsed time.
Finaly we multiply the result by the derivate constant*/

pid_d = kd*((error - previous_error)/elapsedTime);

/*The final PID values is the sum of each of this 3 parts*/
PID = pid_p + pid_i + pid_d;

More is given in the sigma studio material with the comments.

Question: Am I implementing PID control correctly, derivative, integral? I am in need of help. 

SigmaPID - working.rar

zip file added
[edited by: karolina_1 at 5:45 AM (GMT 0) on 1 Jul 2019]