Eine frage Hätte ich noch

undzwar bin ich jetzt dabei ihm das drehen beizubringen daher soll er schauen ob der gegenstand noch im weg ist oder nicht.
er soll aber erst wen er sich gedreht hat wieder schauen ist noch was im weg oder nicht bevor er einfach lossfahrt daher soll er erstmal warten
und er soll auch erst drehen wen der gegenstand länger da ist

giebt es da eine andere möglichkeit als wait um das hinzubekommen? hab es schon mit einem zahler versucht aber das hat nicht so geklapt ist immer gleich wen er sich ein stuck bewegt hat losgebrettert oder hat sich bloss 1 milimeter bewegt

Mein derzeitiger code ist

Code:
 
$prog &HFF , &HF6 , &HD9 , &HFC                             'Standard Fusebits für  Orangutan SV-328

'Die üblichen Definitionen bei Standardprogrammen für Orangutan B-328
$regfile = "m328pdef.dat"
$crystal = 20000000                                         'Quarzfrequenz
$hwstack = 32
$framesize = 64
$swstack = 32
' ------ Anwendungsspezifische Configurationen ---------

Declare Function Srf02_firmware(byval Slaveid As Byte) As Byte
Declare Function Srf02_entfernung(byval Slaveid As Byte) As Integer
Declare Sub Fahren()
Declare Sub Anhalten()
Declare Sub Ruckwarts()
Declare Sub Drehen()

'Ports benennen
Motor1a Alias Portd.6
Motor1b Alias Portd.5
Motor2a Alias Portd.3
Motor2b Alias Portb.3

Config Pind.1 = Output
Config Pinb.3 = Output
Config Pind.3 = Output
Config Pind.5 = Output
Config Pind.6 = Output

Config Scl = Portd.2                                        'Gelb    Ports fuer IIC-Bus
Config Sda = Portd.0                                        'Lila

Config Pind.1 = Output
Red_led Alias Portd.1

Const Srf02_slaveid = &HE0                                  'Standard I2C Adresse von SRF02


'Variablen
Dim I As Integer
Dim A As Integer
Dim I1 As Integer
Dim A1 As Integer

Dim Entfernung As Integer
Dim V As Integer

Dim X As Byte
Dim T1 As Byte
Dim T2 As Byte
Dim T3 As Byte


' PWM Register setzen
' see the ATmega48/168/328P datasheet for detailed register info
' configure for inverted PWM output on motor control pins
Tccr0a = &HF3
Tccr2a = &HF3
' use the system clock / 8 (2.5 MHz) as the timer clock
Tccr0b = &H02
Tccr2b = &H02

Config Timer1 = Timer , Prescale = 8                        'Konfiguriere Timer1
Enable Timer1                                               'schalte den Timer1 ein
On Timer1 Isr_von_timer1                                    'verzweige bei Timer1 überlauf zu   Isr_von_Timer1
Enable Interrupts
Load Timer1 = 3125                                          'Timer1 wert der vorgeladen wird

Main:

                                                   'Warte 3 Sekunden
   I2cinit
   V = 0
   I = 0
   A = 0
   T1 = 0
   T2 = 0
   T3 = 0

   Do
   ' Messen
   If T1 >= 7 Then
      T1 = 0
      Entfernung = Srf02_entfernung(srf02_slaveid)
   End If

   'Fahren oder nicht
      If Entfernung >= 40 Then

         Fahren

       Else

         Anhalten

      End If
   'Beschleunigen

   If T2 >= 50 Then
      T2 = 0
      If I < 50 Then
         I = I + 5
         A = A + 6
      End If
   End If


   Loop

End

Isr_von_timer1:                                             'ISR von Timer1
Load Timer1 = 3125
   T1 = T1 + 1
   T2 = T2 + 1
   T3 = T3 + 1
Return


'------------- Hilfsfunktionen für SRF02 ----------


Function Srf02_firmware(byval Slaveid As Byte) As Byte
Local Firmware As Byte
Local Slaveid_read As Byte

   Slaveid_read = Slaveid + 1

   I2cstart
   I2cwbyte Slaveid
   I2cwbyte 0                                               'Leseregister festlegen
   I2cstop

   I2cstart
   I2cwbyte Slaveid_read
   I2crbyte Firmware , Nack
   I2cstop

   Srf02_firmware = Firmware
End Function



Function Srf02_entfernung(byval Slaveid As Byte) As Integer
Local Lob As Byte
Local Hib As Byte
Local Firmware As Byte
Local Temp As Byte
Local Slaveid_read As Byte

   Slaveid_read = Slaveid + 1

   'Messvorgang in starten
   I2cstart
   I2cwbyte Slaveid
   I2cwbyte 0
   I2cwbyte 81                                              'in Zentimetern messen
   I2cstop

   I2cstart
   I2cwbyte Slaveid
   I2cwbyte 2                                               'Leseregister festlegen
   I2cstop


   I2cstart
   I2cwbyte Slaveid_read
   I2crbyte Hib , Ack
   I2crbyte Lob , Nack
   I2cstop


   Srf02_entfernung = Makeint(lob , Hib)
End Function

' Richtung angeben
Sub Anhalten()

    I = 0
    A = 0

    Ocr2a = 0                                               'Links
    Ocr2b = 0                                               'Links
    Ocr0a = 0                                               'Rechts
    Ocr0b = 0                                               'Rechts

    V = V + 1

    If V = 1000 Then
    Toggle Red_led
    V = 0

    End If

    Return


End Sub

Sub Ruckwarts()

    Ocr2a = I                                               'Links
    Ocr2b = 0                                               'Links
    Ocr0a = 0                                               'Rechts
    Ocr0b = A                                               'Rechts



End Sub

Sub Fahren()

    If T3 >= 7 Then
         T3 = 0
            If Entfernung >= 40 Then
               V = 0
               Ocr2a = 0                                    'Links
               Ocr2b = I                                    'Links
               Ocr0a = A                                    'Rechts
               Ocr0b = 0                                    'Rechts
            End If
         End If
    Return
End Sub