Nemesis
'===============================================================
'Robot cu 2 Motoare si 1 senzor IR pt. detectare de proximitate
'Deplasare la intamplare folosind "faruri" InfraRosii
'Programul compilat ocupa: 176 biti (din 1024 biti)
'===============================================================


'--------------------------------------------------
'Declaratii variabile,etichete,etc.
'--------------------------------------------------

dim StangaIR         'variabile in care se vor stoca valorile
dim DreaptaIR        'senzorului IR pt stanga respectiv dreapta

const Motor1a    8   'motor dreapta
const Motor1b    9   '
const Motor2a   10   'motor stanga
const Motor2b   14   '
const MotorEn   13   'activeaza motoare 

const IRsenzor   5   'senzorul IR de proximitate
const IRmod      6   'modulator

const IRLedSt   11   'IR LED proximitate stanga
const IRLedDr    7   'IR LED proximitate dreapta

const IRmargineS 2   'IR LED margine stanga
const IRmargineD 3   'IR LED margine dreapta

hwpwm 0,50,15        'initializare modulator 40KHz 60% "on time"


'=================================================================
'Programul principal
'=================================================================
  output Motor1a     'Se declara porturile la care sunt conec-
  output Motor1b     'tate motoarele si LED-urile IR ca iesire  
  output Motor2a     '"output" si apoi se seteaza starea
  output Motor2b     'portului sus "high" sau jos "low".
  low Motor1a        'Porturile nedeclarate vor fi automat
  low Motor1b        'setate ca intrare "input".
  low Motor2a
  low Motor2b

  output IRLedSt
  output IRLedDr
  high IRLedSt
  high IRLedDr

  output MotorEn
  high MotorEn

'-----------------------------------------------------------------
'Bucla principala
'-----------------------------------------------------------------
Mainloop:

 '-------------------------------------------------
 'citeste senzorii
 '-------------------------------------------------
  gosub CitireSenzorIR

 '-------------------------------------------------
 'afiseaza in fereastra Debug valorile citite
 '-------------------------------------------------
  print "Stanga ",StangaIR,"  Dreapta ",DreaptaIR

 '-------------------------------------------------
 'decide directia deplasarii
 '-------------------------------------------------
  if DreaptaIR = 0 then  'obstacol detectat la dreapta
    gosub StangaInapoi   'motorul stanga inapoi
  else                   'nimic detectat
    gosub StangaInainte  'motorul stanga inainte
  endif
  if StangaIR = 0 then   'obstacol detectat la stanga
    gosub DreaptaInapoi  'motorul dreapta inapoi
  else                   'nimic detectat
    gosub DreaptaInainte 'motorul dreapta inainte
  endif

goto Mainloop            'mergi la inceputul buclei

'=================================================================
'Subrutine
'=================================================================

'-------------------------------------------------
'Controlul motoarelor
'-------------------------------------------------
DreaptaInainte:
  low Motor1b
  high Motor1a
return

DreaptaInapoi:
  low Motor1a
  high Motor1b
return

StangaInainte:
  low Motor2b
  high Motor2a
return

StangaInapoi:
  low Motor2a
  high Motor2b
return


'-------------------------------------------------
'Citeste senzorul IR de proximitate
'-------------------------------------------------
CitireSenzorIR:
  low IRLedDr               'activeaza LED IR dreapta
  output IRmod              'porneste modulatorul
  pause 1                   'asteapta o milisecunda
  input IRmod               'opreste modulatorul
  DreaptaIR = inp.IRsenzor  'citeste senzorul
  pause 1                   'asteapta o milisecunda
  high IRLedDr              'dezactiveaza LED IR dreapta
  low IRLedSt               'activeaza LED IR stanga
  output IRmod              'porneste modulatorul
  pause 1                   'asteapta o milisecunda
  input IRmod               'opreste modulatorul
  StangaIR = inp.IRsenzor   'citeste senzorul
  pause 1                   'asteapta o milisecunda
  high IRLedSt              'dezactiveaza LED IR stanga
return