Ultraschallsensoren Messung zu langsam?

[Alpha2000]

Hi

Also erstmal zur hardware.
Altes RC auto mit 2 motoren
Orangutan SV-328 Bord
Ultraschall SRF02

Funst auch alles recht gut der code selbst macht was er soll bis darauf das der abstand wo das auto bremst immer unterschiedlich ist. schätze mal das es an meinem code ligt und daran wan er messen tut aber ich weis nicht wie ich es verbessern soll. ist mein erster roboter.

Die derzeitige funktion ist halt Wen nix im weg ist langsam beschleunigen und wen ein gegenstand im weg ist bremsen. ist der gegenstand wieder weg wieder langsam anfahren.

ps. ja die motoren werden unterschiedlich angesteuert da der eine woll ein knacks weg hat und nicht die selbe leistung bringt. so wie er jetzt ist fahrt er gerade aus ungefäht

So hier noch der code ich hoffe ich könnt mir helfen was zu verbessern.

$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 Anhalten()
Declare Sub Ruckwarts()

'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 Entfernung As Integer
Dim V As Byte
Dim X 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

Main:

   Wait 3                                                   'Warte 3 Sekunden
   I2cinit
   V = 1
   I = 0
   A = 0
   Do

   Do

   Entfernung = Srf02_entfernung(srf02_slaveid)
   If Entfernung >= 40 Then

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

   End If
    Waitms 500
      If I < 50 Then
         I = I + 5
         A = A + 6
      End If
   Loop Until I > 50
   Loop



End



'------------- 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


Warteaufmessung:
   Waitms 1
   Firmware = Srf02_firmware(slaveid)
   If Firmware = 255 Then Goto Warteaufmessung

   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


Sub Anhalten()
    I = 0
    A = 0

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

End Sub

Sub Ruckwarts()

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

    Waitms 500
     I = I + 5
     A = A + 8
   Loop Until I > 70


End Sub