So, habe jetzt mal eingebaut dass er mir anzeigt ob er auch tatsächlich was macht oder nicht. Dies ist der Fall
Doch nach Testläufen fährt er exakt so wie ohne Odometrie Programm. Ich gucke mir jetzt mal deinen Vorschlag an, danke schonmal!

// Edit
Hmm irgendwie muss ich doch aber die Variablen initialisieren. Wie mach ich das denn ohne dass die jedes mal zurückgesetzt werden? Das scheint auch das Problem zu sein.