Dude, as I already wrote you in our German forum, you will …
1st of all need to stop the correct loop time which surely is different from 20 (ms) !
dt= currTime- saveTime;
where do you calculate the error (err) – maybe I missed it?
then, what about this line:
integral = integral + (angle * dt);
the correct PID formulas are the following:
err = Sollwert-Zielwert Integr = Integr + err PWMpwr= (KP*err) + (KI*integr)*dt + (KD*(err-errorold))/dt;
then you will finally surely need to tune (and fine-tune) your PID constants!
a hint for your PID regulation:
the loop propably will have to look something like the following:
loop: dt= currTime- saveTime; saveTime= currTime err = Sollwert-Zielwert Integr berechnen (Integr = Integr + err) PWMpwr= (KP*err) + (KI*integr)*dt + (KD*(err-errorold))/dt; errorold=err PWMpwr auf Motoren anwenden dann wieder loop von vorn!