Das schaut für mich von den Interrupts jetzt gar nicht so wild aus. Ich würde vermuten, dass das mit den 16MHz locker geht.
Ich kann nicht genau abschätzen, was die PID-Regelung frisst.