implemented the PID controler, seems to work fine. More testing needed.