Guten Morgen allerseits

Bin neu in die Roboterwelt eingestiegen und auch in die Programmierung,
also geht bitte nicht so hart mit meinen Anfängerfehlern um

Habe einen Roboter gebaut der auf einen myAVR MK2 Board basiert und bin nun an meine Grenzen gestoßen.

Funktion des Roboter ist es in der Gegend um herzufahren und wenn der sharp Ir sensor ein Hindernis erkennt bleibt der Bot stehen, schaut sich um und weicht dem Hindernis aus. (siehe Video)

was man leider nicht auf dem Video sieht, ist das der bot noch ca 15-20 min. Fahrt einfach stehen bleibt und dann nur noch rückwärts fährt. Wahrscheinlich hängt sich das Programm auf. Ich verstehe nur nicht warum das passiert. Vielleicht könnte mal jemand über den Code schauen und mir tips zur Verbesserung geben.

Aber bitte nur Basic/Bascom das ich kein Assembler / C kann
und bei Basic bis jetzt auch nur die Grundfunktionen


Video: http://www.youtube.com/watch?v=rCTV5_ktmKQ



Code:
$regfile = "M168Pdef.dat"                                   'es handelt sich um einen ATmega168P
$crystal = 3686400                                          'der eingebauter RC-Oszillator läuft mit 3,6864 Mhz
$hwstack = 60                                               'im Speicher werden für den Hardware-Stack 60 Byte reserviert
$swstack = 60                                               'im Speicher werden für den Software-Stack 60 Byte reserviert
$framesize = 80                                             'im Speicher werden für den Frame 80 Byte reserviert
$baud = 19200




'IR Sensor
Dim Ir As Word

'Motoren
Dim Mrechts As Byte , Mlinks As Byte

'Servostellung
Dim Servo1 As Byte , X30 As Bit , X40 As Bit , X50 As Bit , X60 As Bit , X70 As Bit , X80 As Bit


'Input für Taster
S1 Alias Pind.2
Smi Alias Pinc.1

'Taster
Config S1 = Input
Config Smi = Input

'PullUp-Widerstand einschalten
S1 = 1
Smi = 1


'Submenüs Declarieren
Declare Sub Vor
Declare Sub Stopp
Declare Sub Denken
Declare Sub Zurück
Declare Sub Weniglinks
Declare Sub Links
Declare Sub Wenigrechts
Declare Sub Rechts
Declare Sub Sehen

'PWM Modulieren
Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Up , Compare B Pwm = Clear Up , Prescale = 8

'Motoren 0 = Vollgas
Mrechts = 0
Mlinks = 0


'Output für PWM
Config Portb.1 = Output
Config Portb.2 = Output


'Motoren
M1vor Alias Portd.4
M1zur Alias Portd.5
M2vor Alias Portd.6
M2zur Alias Portd.7

Config M1vor = Output
Config M1zur = Output
Config M2vor = Output
Config M2zur = Output



'IR Sensor 300 ca 20 cm, 500 ca 10 cm
'Config für Ir Augen
Config Adc = Single , Prescaler = Auto , Reference = Avcc

Start Adc


'Servo Settings
'Servo von 20 - 106 (Verwendet 30-80)
Config Servos = 1 , Servo1 = Portd.3 , Reload = 10
Config Portd.3 = Output

Enable Interrupts

'Servo in Mittelstellung
Servo(1) = 54



'S1 schaltet Robi ein
Bitwait S1 , Reset
Bitwait S1 , Set

Wait 1


'Hauptschleife Go
Haupt:

Do

   Print "Hauptschleife"

  'Mitte  Ca. 54 , 30 Links, 80 Rechts

   Servo(1) = 35

   Call Sehen
   Call Sehen
   Call Sehen
   Call Sehen

   Servo(1) = 75

   Call Sehen
   Call Sehen
   Call Sehen
   Call Sehen

   Call Vor

Loop



End                                                         'end program



Sub Denken

   Print "Denken"

   'Stopp
   M1vor = 0
   M2vor = 0
   M1zur = 0
   M2zur = 0


   Waitms 100
   Servo1 = 30
   Servo(1) = Servo1

   X30 = 0
   X40 = 0
   X50 = 0
   X60 = 0
   X70 = 0
   X80 = 0

   Waitms 300

   Do

      If Smi = 0 Then Call Zurück
      Servo(1) = Servo1
      Waitms 50
      Ir = Getadc(0)
      If Servo1 = 30 And Ir > 210 Then X30 = 1
      If Servo1 = 35 And Ir > 210 Then X30 = 1
      If Servo1 = 40 And Ir > 210 Then X40 = 1
      If Servo1 = 45 And Ir > 210 Then X40 = 1
      If Servo1 = 50 And Ir > 210 Then X50 = 1
      If Servo1 = 55 And Ir > 210 Then X50 = 1
      If Servo1 = 60 And Ir > 210 Then X60 = 1
      If Servo1 = 65 And Ir > 210 Then X60 = 1
      If Servo1 = 70 And Ir > 210 Then X70 = 1
      If Servo1 = 75 And Ir > 210 Then X70 = 1
      If Servo1 = 80 And Ir > 210 Then X80 = 1

      Print "Servopos.: " ; Servo1 ; "  Ir: " ; Ir

      Servo1 = Servo1 + 5

   Loop Until Servo1 > 80



   If X30 = 1 And X40 = 0 And X50 = 0 And X60 = 0 And X70 = 0 And X80 = 0 Then Call Wenigrechts
   If X30 = 0 And X40 = 0 And X50 = 0 And X60 = 0 And X70 = 0 And X80 = 1 Then Call Weniglinks

   If X30 = 1 And X40 = 1 And X50 = 0 And X60 = 0 And X70 = 0 And X80 = 0 Then Call Rechts
   If X30 = 1 And X40 = 1 And X50 = 0 And X60 = 0 And X70 = 0 And X80 = 0 Then Call Rechts
   If X30 = 1 And X40 = 1 And X50 = 1 And X60 = 0 And X70 = 0 And X80 = 0 Then Call Rechts

   If X30 = 0 And X40 = 0 And X50 = 0 And X60 = 0 And X70 = 1 And X80 = 1 Then Call Links
   If X30 = 0 And X40 = 0 And X50 = 0 And X60 = 0 And X70 = 1 And X80 = 1 Then Call Links
   If X30 = 0 And X40 = 0 And X50 = 0 And X60 = 1 And X70 = 1 And X80 = 1 Then Call Links

   If X50 = 1 And X60 = 1 Then Call Zurück
   If X40 = 1 And X70 = 1 Then Call Zurück

   Call Vor

End Sub





Sub Stopp

   Print "Stopp"

   'Stopp
   M1vor = 0
   M2vor = 0
   M1zur = 0
   M2zur = 0

   Goto Denken

End Sub


Sub Vor

   Print "Vor"

   Pwm1a = Mrechts
   Pwm1b = Mlinks

   'Vorwärts fahren
   M1vor = 1
   M2vor = 1
   M1zur = 0
   M2zur = 0

   Goto Haupt

End Sub


Sub Weniglinks

   Print "Weniglinks"

   M1vor = 0
   M2vor = 0
   M1zur = 1
   M2zur = 0

   Waitms 100

   Call Stopp

End Sub


Sub Links

   Print "Links"

   M1vor = 0
   M2vor = 0
   M1zur = 1
   M2zur = 0

   Waitms 200

   Call Stopp

End Sub


Sub Wenigrechts

   Print "Wenigrechts"

   M1vor = 0
   M2vor = 0
   M1zur = 0
   M2zur = 1

   Waitms 100

   Call Stopp

End Sub



Sub Rechts

   Print "Rechts"

   M1vor = 0
   M2vor = 0
   M1zur = 0
   M2zur = 1

   Waitms 200

   Call Stopp

End Sub




Sub Zurück

    Print "Zurück"

    'Zurück
    M1vor = 0
    M2vor = 0
    M1zur = 1
    M2zur = 1

    Waitms 250

    If Ir > 256 Then Call Links
    If Ir < 257 Then Call Rechts

End Sub


Sub Sehen

   If Smi = 0 Then Call Zurück
   If S1 = 0 Then Mrechts = Mrechts + 2

   Ir = Getadc(0)
   Print "Servo: " ; Servo(1) ; "  Ir: " ; Ir
   If Ir > 230 Then Call Stopp
   Waitms 50

End Sub