Also ich würde das ganz anders machen.

Erstmal sollten die Motoren über PWM geschwindigkeits-geregelt werden.
Weiterhin halte ich von Rückwärst fahren (oder auch ein Rad drehen) garnix; außer die Geometrie des Roboters/der Bahn lässt es nciht anders zu.
Dann würde ich aus den 5 Sensoren die mittlere Bahnposition ermitteln.
Also z.B. durchzählen wieviele nebeneinander liegende Sensoren 1 (true, wahr) sind. Dann noch den ersten mit 1 von irgend einer Seite ermitteln und die Hälfte der Anzahl der 1-er Sensoren dazurechnen. Jetzt noch - 2,5 Rechnen, und du hast schonmal nen Wert von -2,5 bis +2,5 in 0,5er Schritten. Jetzt kann man entweder nen linearen regler ansetzen, oder gleich nen PD-Regler. Ob letzterer mit den wenigen Sensordaten gut zurechtkommt, müsste man ausprobieren.

Hoffe mal du hast was verstanden und es geht so; viele Ansätze sind auch über die SuFo zu finden!

MfG; MK