Nemesis
'===============================================================
'Robot cu 2 Motoare si 3 senzori IR pt. detectare de linie
'Program de urmarire de linie
'Programul compilat ocupa: 280 biti (din 1024 biti)
'===============================================================


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


dim Lvalue,Cvalue,Rvalue,Cvalue2,Rvalue2
dim state,dir

const Motor1a    9   'motor dreapta
const Motor1b    8   '
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 3   'IR LED margine stanga
const IRmargineC 2   'IR LED margine centru
const IRmargineD 1   'IR LED margine dreapta


'=================================================================
'Programul principal
'=================================================================
Main:

  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


'---------------------------------------------------------------
'Programul de urmarire de linie
'---------------------------------------------------------------
Linefollowloop:
  Lvalue = inp.IRmargineS    '0 pt.alb, 1 pt negru
  Cvalue = inp.IRmargineC
  Rvalue = inp.IRmargineD

  Cvalue2 = 2*Cvalue
  Rvalue2 = 4*Rvalue
  state = Lvalue + Cvalue2 + Rvalue2

  print "L ",Lvalue,"  C ",Cvalue,"  R ",Rvalue,"  State ",state

  branch state,do0,do1,do2,do3,do4,do5,do6,do7

goto Linefollowloop

do0:        'no sensor over line
  if dir == 0 then
    gosub Tleft
  else
    gosub Tright
  endif
goto Linefollowloop
  
do1:        'left over line
  dir = 0
    gosub Left
goto Linefollowloop

do2:        'center over line
  gosub Fwd
goto Linefollowloop

do3:        'left and center over line
    gosub Tleft
goto Linefollowloop

do4:        'right over line
  dir = 1
    gosub Right
goto Linefollowloop

do5:        'not used - left and right over line
goto Linefollowloop

do6:        'right and center over line
    gosub Tright
goto Linefollowloop

do7:        'all sensors over line
    gosub Stop
goto Linefollowloop



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

'-------------------------------------------------
'Controlul motoarelor
'-------------------------------------------------

Fwd:
  low Motor1b
  low Motor1b
  high Motor1a
  high Motor2a
return

Left:
  low Motor2a
  low Motor1b
  high Motor2b
  high Motor1a
return

Right:
  low Motor2b
  low Motor1a
  high Motor2a
  high Motor1b
return

Tright:
  low Motor1b
  low Motor1a
  low Motor2b
  high Motor2a
return

Tleft:
  low Motor2b
  low Motor2a
  low Motor1b
  high Motor1a
return

Stop:
  low Motor2b
  low Motor2a
  low Motor1b
  low Motor1a
return