Image Hosted by ImageShack.us

   
  Robotic Madness
  J.2.R
 

J.2.R Code



Copy and paste into your stamp editor

J2 Explore Demo

' {$STAMP BS2}
'J2R Scientific
'J2 Explore Demo
'12-5-2005
'The J2 will explore and avoid objects
'it will atempt to reverse and move around what it bumps into.
'J2 will also stop from falling off a desk.
'J2 will also go into a startup program if it is off the ground
'at power up.





val05  VAR  Byte
time   VAR  Word
i      VAR  Byte    'loop counter
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
dt     CON   6      'IR port
INPUT   dt



Main:
PAUSE 200
 val05 = IN6
  IF val05 = 0 THEN start1
PAUSE 200
  PULSOUT NECK, 850
PAUSE 200
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
  PAUSE 100
      IF time < 0062 THEN search
      IF time > 0062 THEN forward
  PAUSE 200
GOTO main


forward:
PAUSE 20
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
  PAUSE 100
  IF time < 0010 THEN escape
  PAUSE 100
     FOR i = 1 TO 30
        val05 = IN6
        IF val05 = 0 THEN stuck
       PULSOUT LEFT, 665
       PULSOUT RIGHT, 865
       PAUSE 20
       NEXT
       PAUSE 20
       val05 = IN6
        IF val05 = 0 THEN stuck
       PAUSE 20
GOTO main


stuck:
  FOR i = 1 TO 20
     PULSOUT LEFT, 769
     PULSOUT RIGHT, 769
     PAUSE 20
     NEXT
    PAUSE 20
  FREQOUT SPKR,200,5000
  PAUSE 200
GOTO escape



search:
    FOR I = 1 TO 10
       PULSOUT LEFT, 865
       PULSOUT RIGHT, 865
       PAUSE 20
       NEXT
    PAUSE 100
       FOR i = 1 TO 20
       PULSOUT LEFT, 565
       PULSOUT RIGHT, 965
       PULSOUT NECK, 750
         val05 = IN6
         IF val05 = 0 THEN stuck
       PAUSE 20
       NEXT
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
  IF time > 0032 THEN turnL
GOTO forward


turnL:
    FOR I = 1 TO 15
       PULSOUT LEFT, 665
       PULSOUT RIGHT, 665
       PAUSE 20
       NEXT
  FREQOUT SPKR,100,1000
  FOR i = 1 TO 40
       PULSOUT LEFT, 565
       PULSOUT RIGHT, 965
       PULSOUT NECK, 850
        val05 = IN6
        IF val05 = 0 THEN stuck
       PAUSE 20
       NEXT
       PAUSE 20
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
   IF time > 0032 THEN forward
GOTO main






escape:
 PAUSE 10
 FREQOUT SPKR,100,1000
 PAUSE 10
    FOR I = 1 TO 25
       PULSOUT LEFT, 965
       PULSOUT RIGHT, 565
       PAUSE 20
       NEXT
PAUSE 20
  FOR i = 1 TO 45
       PULSOUT LEFT, 665
       PULSOUT RIGHT, 665
         val05 = IN6
         IF val05 = 0 THEN stuck
       PAUSE 20
       NEXT
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
   PAUSE 10
   PULSOUT NECK,850
   PAUSE 10
      IF time < 0062 THEN search
      IF time > 0062 THEN forward
GOTO main





start1:
PAUSE 200
 PULSOUT NECK, 900
  PAUSE 200
   FOR i = 1 TO 50
     PULSOUT LEFT, 769
     PULSOUT RIGHT, 769
     PAUSE 20
     NEXT
  PAUSE 200
PAUSE 1000
FREQOUT SPKR,200,5000
PAUSE 100
FREQOUT SPKR,100,1000
PAUSE 100
FREQOUT SPKR,250,4000
PAUSE 1000
GOTO escape

 END


J2SubSumExplore.BS2


' {$STAMP BS2}
'J2SubSumExplore.BS2
'J2R Scientific
'12-12-2005
'J2 will explore about the world using subsumption based
'artificial behavioral based intelligence.




'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
val05  VAR  Byte
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
LED    CON   10          'LED mouth, this is not standard on all J2's



'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
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


'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
LOW LED



main:               'subsumption architecture
  GOSUB wander      'random wander instinct is lowest priority
  GOSUB avoid       '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
'===========================

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,tl,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
  PULSOUT NECK, 900
  LOW LED
  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 > 0062 THEN avdone



  avfront:
  HIGH LED
  avDir = rl                      'rotate away
        avDur = 15
        drive = rl
        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 = rl       '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 FollowDemo.BS2


' {$STAMP BS2}
'J2R Scientific
'J2 Demo New
'12-5-2005
'The J2 will search for an object, follow it, but if it gets too close
'it will beep and atempt to reverse and move around what it bumped into
'then start following again.


val05  VAR  Byte
time   VAR  Word
i      VAR  Byte    'loop counter
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
dt     CON   6      'IR port
INPUT   dt





Main:
     PAUSE 20
        val05 = IN6
        IF val05 = 0 THEN stuck
     PAUSE 20
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
  PAUSE 100
      IF time < 0062 THEN forward
      IF time > 0062 THEN search
  PAUSE 200
GOTO main


forward:
     FOR i = 1 TO 40
       PULSOUT LEFT, 565
       PULSOUT RIGHT, 965
         val05 = IN6
         IF val05 = 0 THEN stuck
       PAUSE 20
       NEXT
       PAUSE 20
       PULSOUT NECK, 850
       PAUSE 100
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
  PAUSE 100
      IF time < 0010 THEN stuck
  PAUSE 100
GOTO main


stuck:
PAUSE 100
   FREQOUT SPKR,200,5000
   PAUSE 20
   FOR i = 1 TO 40
     PULSOUT LEFT, 769
     PULSOUT RIGHT, 769
     PAUSE 20
     NEXT
GOTO escape



search:
    FOR I = 1 TO 6
       PULSOUT LEFT, 865
       PULSOUT RIGHT, 865
       PAUSE 20
       NEXT
    PAUSE 100
       FOR i = 1 TO 20
       PULSOUT LEFT, 565
       PULSOUT RIGHT, 965
       PULSOUT NECK, 900
         val05 = IN6
         IF val05 = 0 THEN stuck
       PAUSE 20
       NEXT
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
  IF time > 0032 THEN turnL
GOTO forward


turnL:
    FOR I = 1 TO 15
       PULSOUT LEFT, 665
       PULSOUT RIGHT, 665
       PAUSE 20
       NEXT
  FREQOUT SPKR,100,1000
  FOR i = 1 TO 40
       PULSOUT LEFT, 565
       PULSOUT RIGHT, 965
       PULSOUT NECK, 900
        val05 = IN6
        IF val05 = 0 THEN stuck
       PAUSE 20
       NEXT
       PAUSE 20
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
   IF time > 0032 THEN search
GOTO main


escape:
    FOR I = 1 TO 30
       PULSOUT LEFT, 965
       PULSOUT RIGHT, 565
       PAUSE 20
       NEXT
       PAUSE 20
       FOR i = 1 TO 25
       PULSOUT LEFT, 665
       PULSOUT RIGHT, 665
       PAUSE 20
       NEXT
       PAUSE 100
  PULSOUT 7, 5
  PULSIN 7, 1, time
  time = time ** 2251
      IF time < 0062 THEN forward
      IF time > 0062 THEN search
GOTO main



 END



J.2.R SubSum Patrol.BS2


' {$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
 
  Today, there have been 16 visitors (51 hits) on this page! Copyright 2007  
 
=> Do you also want a homepage for free? Then click here! <=
Welcome visitor
free counters