PID controller works. Just needs tuning.