Trying to fix PID stuff, encoder stopped working though...