Added a PID controller. Works like a charm. Code is getting a bit furry, needs some upkeep