SRF10 Radar Problem

Hallo,
ich bin inzwischen soweit, dass ich das srf10 auf einem Servomontiert habe. In meinem Programm soll die Umgebung des Servos abgetastet werden, die Position ermittelt werden, an der sich der nächste Gegenstand befindet und zum Schluss soll sich der Servo dorthin drehen und auf dem LCD soll die Entfernung in cm ausgegeben.
Mal davon abgesehen, dass die Entfernung nicht richtig ist (bei ca 4cm bekomme ich einen Wert von ca 4000) habe ich das Problem, dass alles ordungsgemäß funktioniert, wenn das Programm so aussieht:

Code:

---

$regfile = "m16def.dat"                      'Mega16 Defdatei
$crystal = 16000000                          'Quarzfrequenz
$baud = 9600                                'Übertragungsgeschwindigkeit
                                            'für serielle Schnittstelle

'###############################################################
'Initialisierung der Ports
'###############################################################
Config Servos = 1 , Servo1 = Portd.7 , Reload = 10
Config Portd = Output
Config Sda = Portc.1
Config Scl = Portc.0

Declare Function Entf_messung() As Word

Dim Richtung As Byte , Min_richtung As Byte  ' Richtung in der der Gegner sich befindet
Dim Entfernung As Word , Min_entfernung As Word      'Entfernung zum Gegner
Const Slave = &HE0                          'slave address of srf10 (schreiben)
Const Slaver = &HE1                          'slave address of srf10 (lesen)

'##############################################################
'Initialisierung
'###############################################################

Entfernung = 65535
Min_entfernung = 65535
Richtung = 30
Min_richtung = 30

Enable Interrupts
I2cinit
I2cstart                                    'Reichweite festlegen
I2cwbyte Slave
I2cwbyte 2                                  'Register 2 - entfernungsregister
I2cwbyte 20                                  'Befehl 20 - 86cm max Entfernung?
I2cstop

'###############################################################
'Start
'###############################################################


Servo(1) = Richtung                          '
Cls
Lcd "Test"
Wait 1

Waitms 500
For Richtung = 30 To 200 Step 10            'servo drehen
  Servo(1) = Richtung
  Entfernung = Entf_messung()              'Entfernung messen
  If Entfernung < Min_entfernung Then      'überprüfen ob der gegenstand näher ist
      Min_entfernung = Entfernung
      Min_richtung = Richtung
  End If
Next
Servo(1) = Min_richtung                      'Servo auf Objekt ausrichten
Cls
Lcd Str(min_entfernung) ; " cm"              'Ausgabe der Entfernung im LCD
Wait 10
End                                          'end program
'###############################################################
'Funktion Entfernungsmessung
'###############################################################
Function Entf_messung() As Word
Dim Us_data_h As Byte , Us_data_l As Byte    'Bytes zur Entfernungsbestimmung
Dim Us_data As Word                          'Word zur entfernungsbestimmung
I2cstart                                    'Messung auslösen
I2cwbyte Slave
I2cwbyte 0                                  'Register 0 - Befehlsregister
I2cwbyte 81                                  'Befehl 81 - Messung in cm
I2cstop

Waitms 85                                    'delay 85mS auf Ergebnis warten

I2cstart
I2cwbyte Slave                              'Lesen beginnen E0
I2cwbyte 2                                  'Reg 2 => Entfernung
I2cstop
I2cstart                                    'Leseadresse (E1)
I2cwbyte Slaver                              'Addresse vom srf 10
I2crbyte Us_data_h , Ack                    'beide reister (2+3) auslesen
I2crbyte Us_data_l , Nack
I2cstop

Entf_messung = Makeint(us_data_h , Us_data_l)      'Daten in Word wandeln
Cls
Lcd Entf_messung
Waitms 500
End Function

---

Wenn ich aber die Befehle:
Cls
Lcd Entf_messung
Waitms 500
in der Funktion auskommentiere, dann dreht sich der Servo stets auf die Startposition und auf dem LCD erscheint "0 cm".
Warum? Was ist falsch?
Danke Gruß
Stefan