Hallo Simon, schau dir deinen Code Mal ganz genau an - Wo erfasst du die Messdaten?

Außerdem machst du in deiner Schleife eine Reihe sinnloser Dinge - Du setzt die Geschwindigkeit und Richtung einmal pauschal am Anfang und überschreibst sie direkt danach wieder.
Außerdem würde ich am ehesten die beiden Werte miteinander vergleichen und nicht mit einer Konstanten ...

mfG
Markus