Nem tens de colocar delay, olha para o exemplo blinky without delay, não deves andar a executar o PID sem aplicar os novos valores de pwm nos motores, nem precisas de ter o pid a correr a centenas de Khz porque vai-te criar oscilações no sistema, o teu KP parece-me estranhamente pequeno.
