die beschreibung klingt nach ziemlichem Überschwingen *g*
wie hast du Kp denn gewählt? und wie berechnest du e?

hast du eine möglichkeit ide zykluszeit (deutlich) zu verringern?
1s ist ne ewigkeit, wenn du einen Motor regeln willst.