Reworked the code and cleaned it up a bit even though there's plenty left to do. PID-control doesn't work as it should