J.2.R Code

---

' {$STAMP BS2}
'J2SubSumPatrol.BS2
'J2R Scientific
'12-11-2005
'This will allow the J2 to patrol in a short pattern
'If something is too close in the patrol route
'J2 will sound an alarm and slow come towards the object.




'Generic values
tmp    VAR  Word         'tmp var, many routines
'ltmp   VAR  tmp.BYTE0    'use these tmp variables
'htmp   VAR  tmp.BYTE1    'be careful with them
seed   VAR  Word         'random number seed
time   VAR  Word
i      VAR  Byte         'loop counter
a      VAR  Byte
PING   CON   7           'PING sonar port
LEFT   CON   3           'left wheel port
RIGHT  CON   1           'right wheel port
NECK   CON   2           'neck servo port
SPKR   CON   8           'speaker port



'These are for the servo routines
SACT  CON  5               'times through act routine
drive  VAR  Word           'wheel command combo
ldrive  VAR  drive.BYTE1   'left wheel command
rdrive   VAR  drive.BYTE0  'right wheel command
aDur    VAR     Nib        'duration of pulsout

'normal list follows
rv  CON  $6432      'forward
fd  CON  $3264      'reverse
st  CON  $4d4d      'stop
tr  CON  $324d      'turn right
tl  CON  $4d64      'turn left
rr  CON  $6464      'rotate right
rl  CON  $3232      'rotate left
bl  CON  $324d      'backup turning left

'wander values
wDir  VAR  Word      'wander value
wDur  VAR  Byte      'wander duration


'avoid states and vars
avDir  VAR  Word      'direction
avDur  VAR  Nib      'duration


'bumper vars and constants
bumper  VAR  IN6    'bumper io pin
bstate  VAR  Nib    'bumper FSM state
bDir  VAR  Word      'bumper direction holder
bDur  VAR  Byte      'duration in that direction


'set up for running
wDur = 0        'clear wander duration
aDur = 0        'clear act loop counter
bDur = 1        'clear bumper duration, may need to change back to 0
bstate = 0        'state
drive = st        'stop servo motors - not really needed



main:               'subsumption architecture
  GOSUB patrol      'patrol in a set pattern
  GOSUB attack       'avoid running into stuff
  GOSUB bumpck      'don't stay bumped into it = highest priority
  GOSUB act         'acts on highest priority movement needed
                    ' i.e. last to set direction
  GOTO main

'===========================
'    Behaviors
'===========================


patrol:                'patrol in a pattern
  IF wDur > 0 THEN wDone1
  RANDOM seed          'random direction
  i = seed & %111      'mask off for 0-7 only s
  LOOKUP i,[st,fd,st,rr],wDir'chose direction
  seed = seed + i      'why is this????
  wDur =  i + 30       'mask for 64 choices of duration

wDone1:
  wDur = wDur - 1      'decrement wander counter
  drive = wDir         'get direction
  PULSOUT NECK, 900
  RETURN               'completed


act:                       'moves servo motors
  IF aDur > 0 THEN aDec    'already doing one, got here
  aDur = SACT              '# of main loops between pulseouts +1
  PULSOUT LEFT,ldrive * 10
  PULSOUT RIGHT,rdrive * 10
aDec:  aDur = aDur - 1     'decrement act loop cntr
  RETURN



attack:                    'charge towards object and beep
 PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
    IF time > 0032 THEN avdone

  FREQOUT SPKR,200,5000
  avfront:
  avDir = fd
        avDur = 15
        drive = fd
        GOTO avdone


avdone:
RETURN





bumpck:                      'Bumper reaction Finite State Machine (FSM)
  IF bumper = 0 THEN bmpnow  'Being bumped now! Restart state machine
  IF bDur > 0 THEN bmpact    'not done current state yet, continue bump
                             ' action set by going into current state
                             'current state finished, set next state
  BRANCH bstate,[bDone1,bbup]  'jump to states 0-1, 2 immed. follows

breset:           'end state 2, now reset
  bstate = 0      'state machine to idle
  RETURN

bbup:             'end state 1, now
  bDir = tl       'rotate left away
  bDur = 65       'sets time limit
  bstate = 2      'next state
  GOTO bdrive

bmpnow:           'being bumped now!
  bDir = rv       'set backup while bumped and
  bDur = 61       'for a while (+1) after not being bumped
  bstate = 1      'start state machine

bmpact:              'bump mode active
  bDur = bDur - 1    'decrement bump timer

bdrive:
  drive = bDir       'set drive direction to bump

bDone1:              'no bump and state machine not running
  RETURN

END

---

J.2.R SubSum Explore.BS2   With two Ping sonars


' {$STAMP BS2}
'J2SubSumExplore.BS2
'J2R Scientific
'8-8-2006
'J2 will explore about the world using subsumption based
'artificial behavioral based intelligence and 2 PING sonar units.




'Generic values
seed   VAR  Word         'random number seed
time   VAR  Word
time2  VAR  Word
i      VAR  Byte         'loop counter
a      VAR  Byte
PING   CON   6           'PING sonar port (right)
PING2  CON   7           'PING sonar port (left)
LEFT   CON   1           'left wheel port
RIGHT  CON   0           'right wheel port
'SPKR   CON   8           'speaker port
CdS     CON   14
light   VAR   Word


'These are for the servo routines
SACT  CON  5               'times through act routine
drive  VAR  Word           'wheel command combo
ldrive  VAR  drive.BYTE1   'left wheel command
rdrive   VAR  drive.BYTE0  'right wheel command
aDur    VAR     Nib        'duration of pulsout

'normal list follows
rv  CON  $6432      'reverse
fd  CON  $3264      'forward
st  CON  $4d4d      'stop
tr  CON  $324d      'turn right
tl  CON  $4d64      'turn left
rl  CON  $6464      'rotate right
rr  CON  $3232      'rotate left
bl  CON  $644d      'backup turning left

'wander values
wDir  VAR  Word      'wander value
wDur  VAR  Byte      'wander duration

'avoid states and vars
avDir  VAR  Word      'direction
avDur  VAR  Nib      'duration
avDir2 VAR Word
avDur2 VAR Nib

'set up for running
wDur = 0        'clear wander duration
aDur = 0        'clear act loop counter
avDur = 0
avDur2 = 0
drive = st        'stop servo motors - not really needed



main:               'subsumption architecture
  GOSUB seeLight
  GOSUB wander      'random wander instinct is lowest priority
  GOSUB avoid       'avoid running into stuff
  GOSUB avoid2
  GOSUB act         'acts on highest priority movement needed
                    ' i.e. last to set direction
  GOTO main

'===========================
'    Behaviors
'===========================

wander:                'randomly wander around
  IF wDur > 0 THEN wDone1
  RANDOM seed          'random direction
  i = seed & %111      'mask off for 0-7 only
  LOOKUP i,[fd,bl,fd,fd,fd,fd,tr,fd],wDir 'chose direction
  seed = seed + i
  wDur = (seed & %111111) + 20  'mask for 64 choices of duration
wDone1:
  wDur = wDur - 1      'decrement wander counter
  drive = wDir         'get direction
  RETURN               'completed

act:                       'moves servo motors
  IF aDur > 0 THEN aDec    'already doing one, got here
  aDur = SACT              '# of main loops between pulseouts +1
  PULSOUT LEFT,ldrive * 10
  PULSOUT RIGHT,rdrive * 10
aDec:  aDur = aDur - 1     'decrement act loop cntr
  RETURN



avoid:
 PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
       IF time < 0052 THEN avfront
       RETURN

 avfront:
  avDir = rl                      'rotate away
        avDur = 30
        drive = avDir
        GOTO avdone

avdone:
RETURN


avoid2:
 PULSOUT 6, 5
  PULSIN 6, 1, time2
  time2 = time2 ** 2251
    IF time2 < 0052 THEN avfront2
    RETURN

 avfront2:
  avDir2 = rr                      'rotate away
        avDur2 = 30
        drive = avDir
        GOTO avdone2

avdone2:
RETURN


seeLight:
 HIGH 14
 'PAUSE 1
  RCTIME CdS,1,light
   DEBUG HOME,DEC5 light,CR
  IF light < 0100 THEN seeAv
 RETURN


seeAv:
 FOR a = 1 TO 25
       PULSOUT LEFT, 965
       PULSOUT RIGHT, 565
       PAUSE 20
       NEXT
 PAUSE 20
  FOR a = 1 TO 45
       PULSOUT LEFT, 665
       PULSOUT RIGHT, 665
       PAUSE 20
       NEXT
    RETURN




END