PID Run Away Motor Control
Try changing the line that reads

self-&gt;integral += self-&gt;error;

to

self-&gt;integral += self-&gt; Ki * self-&gt;error;

and match that by changing the line that reads

self-&gt;output = (self-&gt;Kp*self-&gt;error) + (self-&gt;Ki * self-&gt;integral) + (self-&gt;Kd * derivative);

to

self-&gt;output = (self-&gt;Kp*self-&gt;error) + self-&gt;integral + (self-&gt;Kd * derivative);

That will scale the integral term correctly for your integrator limiting step.