$regfile = "m128def.dat"

Config Timer1 = Pwm , Pwm = 9 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 256

Tccr1a = 0
Tccr1a.com1a1 = 1 : Tccr1a.com1a0 = 0
Tccr1a.com1b1 = 1 : Tccr1a.com1b0 = 0
Tccr1b.wgm13 = 1 : Tccr1b.wgm12 = 1 : Tccr1a.wgm11 = 1 : Tccr1a.wgm10 = 0

Capture1 = 777                                              ' 40 Hz * 1024/777 = 50 Hz

Ddrd = &B11100000
Config Pind.4 = Input
Config Pind.5 = Input
Config Pind.3 = Output
Dataout Alias Portd.3                                       ' Az infraszenzor alatti szervo portja
Sbal Alias Pind.4                                           ' A baloldali mechanikus erzekelo portja
Sjobb Alias Pind.5                                          ' A jobboldali mechanikus erzekelo portja


Declare Sub Haladj(byval Ido As Long)
' A robot max. sebeséggel halad elore a megadott ido eltelteig

Declare Sub Haladji(byval Balseb As Integer , Byval Jobbseb As Integer , Byval Ido As Long)
' A bal kerek "balseb", a jobb pedig "jobbseb" sebesseggel forog "ido" ideig, ahol a sebesseg
' -20..20 lehet !!!

Declare Sub Tolass(byval Ido As Long)
' A robot tolat max. sebeséggel "ido" ideig

Declare Sub Jobbra(byval Ido As Long)
' A robot folyamatosan jobbra forog "ido" ideig

Declare Sub Balra(byval Ido As Long)
' A robot folyamatosan balra forog "ido" ideig

Declare Sub Allj
' A robot megáll

Declare Function Korulnez() As Word
' Megadja, hogy az infraszenzor melyik pozicioban lat el legmesszebre
' A visszateresi ertek: 120 es 500 kozott van

Declare Sub Radar(byval Pos As Word , Byval Ido As Long)
' Az infraszenzor elkezd pozicionalni "Pos" poziciora "ido" ideig
' Kb. ido = 20 eseten varhatoan sikerul neki elerni a kivant poziciot
' Pos = 120..500

Config Adc = Single , Prescaler = 32 , Reference = Internal
Start Adc

Dim _time As Long , D1 As Word , D2 As Word , Tav As Word
Dim Delta As Long

'*******************************************************************************
'                                   FOPROGRAM
'*******************************************************************************

Do
  Call Radar(310 , 20)
  If Getadc(0) > 400 Then
    Call Tolass(30000)
    Call Allj
    Tav = Korulnez()
    Delta = Tav - 310
    Delta = Abs(delta)
    If Tav < 310 Then Call Jobbra(delta) Else Call Balra(delta)
  Else
    Call Radar(270 , 20)
    D1 = Getadc(0)
    Call Radar(350 , 20)
    D2 = Getadc(0)
    If D1 < D2 Then Call Haladji(20 , 5 , 30000) Else Call Haladji(5 , 20 , 30000)
  End If
Loop

'*******************************************************************************
' Szubrutinok
'*******************************************************************************

Declare Sub Csap                                            'figyeli a ket csapot (mechanikus erzekelot)

Sub Csap
   If Sbal = 0 Then
        Call Tolass(30000) : Call Jobbra(1000) : Goto Ki
   End If
   If Sjobb = 0 Then
        Call Tolass(30000) : Call Balra(1000) : Goto Ki
   End If
   Ki:
End Sub

Declare Sub Indulj                                          ' PWM-et bekapcsolja

Sub Indulj()

Capture1 = 777

End Sub

Sub Haladj(byval Ido As Long)

Call Indulj
Compare1a = 75
Compare1b = 35
For _time = 1 To Ido : Call Csap : Next _time

End Sub

Sub Haladji(byval Balseb As Integer , Byval Jobbseb As Integer , Byval Ido As Long)

Call Indulj
Balseb = Balseb Mod 21
Jobbseb = Jobbseb Mod 21
Compare1a = 55 + Balseb
Compare1b = 55 - Jobbseb
For _time = 1 To Ido : Call Csap : Next _time

End Sub

Sub Tolass(byval Ido As Long)

Call Indulj
Compare1a = 35
Compare1b = 75
For _time = 1 To Ido : Call Csap : Next _time

End Sub

Sub Jobbra(byval Ido As Long)

Call Indulj
Compare1a = 75
Compare1b = 75
For _time = 1 To Ido : Call Csap : Next _time

End Sub

Sub Balra(byval Ido As Long)

Call Indulj
Compare1a = 35
Compare1b = 35
For _time = 1 To Ido : Call Csap : Next _time

End Sub

Sub Allj()

Capture1 = 10

End Sub

Function Korulnez() As Word
   Dim P As Word , W As Word
   Dim Minsv As Word , Minsvi As Word

   Call Radar(120 , 15)

   Minsv = 1000
   Minsvi = 120

   For P = 120 To 500 Step 8
      Call Radar(p , 6)
      W = Getadc(0)
      If W < Minsv Then
        Minsv = W
        Minsvi = P
      End If
   Next P
   Call Radar(310 , 15)
   Reset Dataout
   Korulnez = Minsvi
End Function

Sub Radar(byval Pos As Word , Byval Ido As Long)
   Dim P2 As Word , J As Word
   P2 = 5000 - Pos
   For _time = 1 To Ido
      Set Dataout : For J = 0 To Pos : Next J
      Reset Dataout : For J = 0 To P2 : Next J
      Call Csap
   Next _time
End Sub
