Image Hosted by ImageShack.us

   
  Robotic Madness
  Robotic Base RSV2
 

Robotic Base RSV2

Copy and paste this into your Stamp editor



'{$STAMP BS2p}
This shows the Subsumption engine in the Stamp 2p40.
' It uses the ramping servo controllers and sensors from the Co-Processor
'




' -----[ I/O Definitions ]------------------------------------------------------

To_Co_Proc  CON   10    ' pin for data to BBDI Co-Processor
Frm_Co_Proc  CON   9    ' pin for data from BBDI Co-Processor
Timer_Exp  VAR  IN11    ' Timer Expired I/O pin from BBDI Co-Processor
BAUD    CON  240    ' 2400 BAUD => BS2 = 396, BS2p = 1021 = CoProc V1.0
          ' 9600 BAUD => BS2 = 84, BS2p = 240
          ' CoProc V1.1 pin 21 pulled up = 2400 Baud
          ' CoProc V1.1 pin 21 grounded = 9600 Baud
LF     CON   10    ' Line Feed

' -----[ Constants ]------------------------------------------------------------

bot_control  CON   $D5    ' this byte controls the Subsumption Architecture.
          '   it is programmable in Co-Processor
'  bot_control is also defined below as its separate named bits for clarity here
'     D = Ball Bearing = 1; Show Vision = 1; level 5 = 0; do_bumpers = 1;
'    5 = level 3 = 0; do_vision = 1; level 1 = 0; do_bot = 1


Ball_Bearing  CON   1    ' Ball Bearing determines the relative directions
          ' vs servo pulse width. Libby has ball bearing
          ' servos so it is set to 1. This program will
          ' not show Ball Bearing usage.

Show_Vision  CON   1    ' IF enabled shows the vision output on chan 6 and 7
level_5    CON   0    ' Level 5 is like Level 1 except highest priority
do_bumpers  CON   1    ' bumpers are at level 4

level_3    CON   0    ' Level 3 is like Level 1 except higher priority
do_vision  CON   1    ' enable vision from the IR Proximity Detector
level_1    CON   0    ' you write the direction and duration for level 1
          ' and it gets integrated into the Subsumption
          ' calculations.
          ' The lowest level is randomly wandering around.
          ' it is always enabled
do_bot    CON   1    ' enables the subsumption engine.

          'Values shown are initialization values
LEFT_STOP  CON   127    'Left Stop - reprogrammable in Co-Processor
RIGHT_STOP  CON   127    'Right Stop - reprogrammable in Co-Processor
LEFT_MIN  CON   0    'Left Min - reprogrammable in Co-Processor
RIGHT_MIN  CON   0    'Right Min - reprogrammable in Co-Processor
LEFT_MAX  CON   255    'Left Max - reprogrammable in Co-Processor
RIGHT_MAX  CON   255    'Right Max - reprogrammable in Co-Processor

' DIRECTION VALUES WRITTEN INTO THE DRIVE REGISTER BY THE VARIOUS BEHAVIORS
'Direction, bit3, bit2, bit1, bit0
'xxyy
'xx = 00 -> left motor stopped
'xx = 01 -> left motor forward
'xx = 10 -> left motor backward
'xx = 11 -> left motor forward

'yy = right motor

'normal list follows
fd  CON  %0101      'forward
rv  CON  %1010      'reverse
st  CON  %0000      'stop
tr  CON  %0100      'turn right
tl  CON  %0001      'turn left
rr  CON  %0110      'rotate right
rl  CON  %1001      'rotate left
bl  CON  %1000      'backup turning left


' -----[ Variables ]------------------------------------------------------------

SerDIn    VAR    Byte   'gets serial data back from Co-Proc
ServVal_0   VAR    Byte   'to load servo 0 delays
ServVal_0_Old   VAR    Byte   'remember old servo 0 delays
ServVal_1   VAR    Byte   'to load servo 1 delays
ServVal_1_Old   VAR    Byte   'remember old servo 1 delays
ramp_0     VAR    Byte   'servo 0 ramp rate
ramp_1     VAR    Byte   'servo 1 ramp rate


Drive    VAR    Byte   'each behavior subroutine writes direction here
wDir    VAR    Byte   'Wander Direction
wDur    VAR    Byte   'Wander Duration
IRPDDir    VAR    Byte   'IRPD Direction
IRPDDur    VAR    Byte   'IRPD Duration
bDir    VAR    Byte   'Bump Direction
bDur    VAR    Byte   'Bump Duration
L1_Dir    VAR    Byte   'Level 1 Direction
L1_Dur    VAR    Byte   'Level 1 Duration
L3_Dir    VAR    Byte   'Level 3 Direction
L3_Dur    VAR    Byte   'Level 3 Duration
L5_Dir    VAR    Byte   'Level 5 Direction
L5_Dur    VAR    Byte   'Level 5 Duration

i    VAR    Byte   'used by IRPD
seed    VAR  Word  'random number seed

ir_left    VAR  SerDIn.BIT5  '1 = IR sensed something on left side
ir_right  VAR  SerDIn.BIT6  '1 = IR sensed something on right side

bumper_Left  VAR  Bit  '1 = IR sensed something on left side
bumper_Right  VAR  Bit  '1 = IR sensed something on right side
first_bump  VAR  Bit  'tells first impact for immediate stop
bstate    VAR  Nib  'holds state of bumper FSM

' -----[ Initialization Code ]--------------------------------------------------


  ServVal_0 = LEFT_STOP + 1    ' need a value different from what
  ServVal_0_Old = LEFT_STOP + 2      ' is to be sent
  ServVal_1 = RIGHT_STOP + 1
  ServVal_1_Old = RIGHT_STOP + 2
  ramp_0 = 24         '6 counts/20ms
  ramp_1 = 24         '6 counts/20ms
  first_bump = 0
  bumper_Left = 0
  bumper_Right = 0


  AUXIO           '2p40 processor only!

  SEROUT To_Co_Proc, BAUD, [116]
  SEROUT To_Co_Proc, BAUD, [116]     'Reset Co-Processor
            'Reset turns servos off
            ' and turns ramping off

'  We would normally initialize for robot operation. That would start the IRPD
'   (IR Proximity Detect). Here we don't want the Subsumption Engine to start.
'  We just want to turn on the IRPD so we can look at what is happening.
'  Notice that the IRPD LEDs can still light IF you set the bot_control.

  SEROUT To_Co_Proc, BAUD, [214,23]  'write IR Freq Register
              'default = 23; max sensitivity = 27; min = 20
              'Writing a nonzero value enables the IRPD
              'period = 15.6 + 0.4*counts (us) (23 = 24.8uS)
              'Change the 23 to other values and see the effect
              ' on sensitivity.

  SEROUT To_Co_Proc, BAUD, [(218)]    'command to write bot_control
  SEROUT To_Co_Proc, BAUD, [$40]      'run the IRPD display LEDs but not the robot.
              'without this command the display LEDs (ch 6 and 7)
              '   won't light.
    ' D = Ball Bearing = 0; Show Vision = 1; level 5 = 0; do_bumpers = 0;
    ' 5 = level 3 = 0; do_vision = 0; level 1 = 0; do_bot = 0


  SEROUT To_Co_Proc, BAUD, [144,ramp_0]   'write ramp rate to servo 0
  SEROUT To_Co_Proc, BAUD, [145,ramp_1]   'write ramp rate to servo 1
            'Ramp rate of x will incr or decr
            ' pulsewidth by x/4 per 20 ms
            ' up to a max of x = 31
            'write ramp command = 144 + ch#

' -----[ Main Code ]------------------------------------------------------------

Main:           'subsumption architecture
  GOSUB Rd_Sensors       ' check bumpers and vision values
            ' takes 9.6 ms (mostly because
            '  2 bytes @ 2400 Baud = 8.33ms)
            ' (9600 Baud will reduce by over 6ms)

  IF do_bot = 0 THEN no_subsum:    ' subsumption engine enabled?
            '  subsumption takes 5.8 ms to execute
            'Below are the behaviors (in subroutines)
  GOSUB wander         'do the random wander about subroutine (Level0)

            ' wander is lowest priority
  GOSUB Level1         'do user subroutine
  GOSUB IRPD         'do the IRPD subroutine (Level2)
  GOSUB Level3         'do user subroutine (not shown)
  GOSUB bumpck         'do the bumper subroutine (Level4)
  GOSUB Level5         'do user subroutine (not shown)
            ' Level5 is highest priority


  GOSUB action         'figure out servo values from results of
            ' previous subroutines
no_subsum:
      PAUSE 5        'NEEDED TO KEEP LOOP AT ROUGHLY 20 MS FOR DURATION COUNTERS
            ' DECREMENT IN THE RIGHT AMOUNT OF TIME
            '  9.6 ms + 5.8 ms = 15.4 ms -> need 5 more
            '  for 20.4 ms period
  GOTO Main         'keep looping doing it

' -----[ Subroutines ]----------------------------------------------------------

Rd_Sensors:        ' reading the sensors takes 9.6 ms,
          ' of that, 8.3 ms is the serial transfer

'structure of bot_status byte from the CoProcessor
'bstate1    VAR  BIT    'Bumper Finite State Machine = LSB
'bstate2;
'BumperFlg_Leftbmp  VAR  BIT    'bumper hit
'BumperFlg_Rightbmp;
'first_bump    VAR  BIT    'bumper just hit - to stop NOW!
            ' N/A if robot mode isn't running
'see_on_left    VAR  BIT    'vision sees something on left
'see_on_right    VAR  BIT    'vision sees something on right
'bit7      VAR  BIT    'NOT USED = MSB


'  read_CoProc        'read the value from the CoProcessor

  SEROUT To_Co_Proc, BAUD, [118]    'Send the byte command to read robot status
  SERIN Frm_Co_Proc, BAUD, [SerDIn]  'Data comes back into SerDIn

            'since ir_right is defined as SerDIn.BIT6 and
            ' ir_left is defined as SerDIn.BIT5,
            ' they are set by the definitions


  IF (SerDIn & $08)=0 THEN chk_bmp_L

  bumper_Right = 1    '1 = hit something on right side - bit is sticky
          ' it is reset in bumper FSM
chk_bmp_L:

  IF (SerDIn & $04) = 0 THEN done_sense
  bumper_Left = 1      '1 = hit something on left side bit is sticky
          ' it is reset in bumper FSM
done_sense:
  RETURN          'completed


' -----

wander:          ' The lowest level is randomly wandering around.
          ' it is always enabled

  IF wDur > 0 THEN wDone0
  RANDOM seed        'random direction
  LOOKUP (seed & %111),[fd,tl,fd,fd,fd,tr,fd,fd],wDir
  RANDOM seed        'random duration
  wDur = (seed & %1111111)    'mask for 128 choices of duration
  IF wDir = fd THEN add20
  wDur = wDur & %111111      'mask turns down to 64 choices of duration

add20:
  wDur = wDur + 20      'add 400ms for a minimum duration
wDone0:
  wDur = wDur - 1        'decrement wander counter
  drive = wDir        'get direction
  RETURN          'completed

' -----

Level1:         'do user subroutine
          ' you write the direction and duration for level 1
          ' and it gets integrated into the Subsumption
          ' calculations.
  IF level_1 = 0 THEN Done1      ' not enabled
  IF L1_Dur = 0 THEN Done1    ' not active now - timed out
  L1_Dur = L1_Dur - 1      ' decrement duration counter
  drive = L1_Dir        ' get direction
Done1:
  RETURN          ' completed

' -----

IRPD:         'do IRPD Vision subroutine
  IF do_vision = 0 THEN IRPDdone  ' not enabled
          'decide on movement direction always
  i = ir_left * 2 + ir_right    '1=right, 2=left, 3=both
  BRANCH i,[IRPDDec,IRPDleft,IRPDright,IRPDfront]  'if IR left -> go right

IRPDfront:
  IRPDDir = rl      'rotate left away
          ' internal CoProcessor code rotates randomly either direction
  IRPDDur = 16      'Duration + 1
  drive = IRPDDir
  RETURN

IRPDleft:
  IRPDDir = tl
'  IRPDDur = 10      'same # as IRPDright - so share
  GOTO IRPDsdur

IRPDright:
  IRPDDir = tr      'turn right
IRPDsdur:
  IRPDDur = 10      'Duration + 1
'  goto IRPDDec      'falls through

IRPDDec:          'decrement current one
  IF IRPDDur = 0 THEN IRPDdone  'no IRPD move in progress
  IRPDDur = IRPDDur - 1
IRPDDrv:
  drive = IRPDDir

IRPDdone:
  RETURN

' -----

Level3:         'do user subroutine
          ' Level 3 is like Level 1 except higher priority
  IF level_3 = 0 THEN Done3      ' not enabled
  IF L3_Dur = 0 THEN Done3    ' not active now - timed out
  L3_Dur = L3_Dur - 1      ' decrement duration counter
  drive = L3_Dir        ' get direction
Done3:
  RETURN          ' completed

' -----

bumpck:          'do bumper subroutine  (= Level4)
          'Bumper reaction Finite State Machine (FSM)
            ' State 0 = idle, just checking IF bumper is hit
            ' State 1 = currently backing up from hit
            ' State 2 = rotating away

  IF do_bumpers = 0 THEN bDone4    ' not enabled

  IF SerDIn.BIT2 = 1 THEN bmpnow  'Being bumped on left now! Restart state machine
  IF SerDIn.BIT3 = 1 THEN bmpnow  'Being bumped on right 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,[bDone4,end_st1]  'jump to states 0-1, state 2 immed. follows

  IF i < 3 THEN breset  'if both IR sensors aren't lit, done spinning

keepspin:
  bDur = 20    'something is still in front, need to keep spinning
  GOTO bdrive

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

end_st1:      'end state 1, now

  IF bumper_Left = 0 THEN rtbmp
  bDir = rr      'rotate right away from left bump
  GOTO bmpdur
rtbmp:
  bDir = rl      'rotate left away from right bump
          'internal CoProcessor code is random direction if both bumpers hit
bmpdur:
  bDur = 25      'internal CoProcessor code is random from 0.2 to 0.5 seconds
  first_bump = 0      'reset sticky bits
  bumper_Left = 0
  bumper_Right = 0
  bstate = 2      'next state
  GOTO bdrive


bmpnow:            'being bumped now!
  IF first_bump = 1 THEN still_bmp    'still being bumped?
  first_bump = 1          'capture first bump only
  SEROUT To_Co_Proc, BAUD, [152,LEFT_STOP]   'write position to servo 0 - immediate stop!
  SEROUT To_Co_Proc, BAUD, [153,RIGHT_STOP]   'write position to servo 1 - immediate stop!
              ' trick - will over-ride ramping to stop but
              ' start ramping to the new value from there.
still_bmp:
  bDir = rv      'set backup while bumped and
  bDur = 18      'for a while (+1) after not being bumped
          '  internal CoProcessor code duration is shorter (10) if
          '  only one bumper side is hit
  bstate = 1      'start state machine

bmpact:        'bump mode active
  bDur = bDur - 1    'decrement bump timer
bdrive:
  drive = bDir    'set drive direction to bump

bDone4:        'no bump and state machine not running - done level 4
  RETURN      ' completed

' -----

Level5:         'do user subroutine
          ' Level 5 is like Level 1 except highest priority
  IF level_5 = 0 THEN Done5    ' not enabled
  IF L5_Dur = 0 THEN Done1    ' not active now - timed out
  L5_Dur = L5_Dur - 1      ' decrement duration counter
  drive = L5_Dir        ' get direction
Done5:
  RETURN          ' completed

' -----

action:            'moves servo motors
            'uses ramping servo controller for simplicity
            'servo 0 = Left, Servo 1 = Right
            ' Ball Bearing assumed to be 1 (for Libby)
            ' Ball Bearing being 0 reverses MINs and MAXes
            ' see Subsume_BoeBot1.BS2


  IF drive.BIT2 = 1 THEN lftfwd    'jump if want to move left motor forward
  IF drive.BIT3 = 1 THEN lftbak    'jump if want to move left motor backward

  ServVal_0 = LEFT_STOP
  GOTO lft_pulse

lftbak:
  ServVal_0 = LEFT_MAX      'MIN is backward when BB = 0
  GOTO lft_pulse

lftfwd:
  ServVal_0 = LEFT_MIN      'MAX is forward when BB = 0

lft_pulse:
  IF (ServVal_0 = ServVal_0_Old) THEN chkright
            'Only need to send servo data
            ' IF different from last time.
  SEROUT To_Co_Proc, BAUD, [136,ServVal_0] 'Set Servo 0 = Left side
  ServVal_0_Old = ServVal_0     'remember value sent

chkright:
  IF drive.BIT0 = 1 THEN rtfwd    'see comments for left side above
  IF drive.BIT1 = 1 THEN rtbak    'MIN MAX are reversed to reverse
            ' servo direction on other side

  ServVal_1 = RIGHT_STOP
  GOTO rt_pulse

rtbak:
  ServVal_1 = RIGHT_MIN
  GOTO rt_pulse

rtfwd:
  ServVal_1 = RIGHT_MAX

rt_pulse:
  IF (ServVal_1 = ServVal_1_Old) THEN DoneServ
            'Only need to send servo data
            ' IF different from last time.
  SEROUT To_Co_Proc, BAUD, [137,ServVal_1] 'Set Servo 1 = Right side
  ServVal_1_Old = ServVal_1     'remember value sent
DoneServ:
  RETURN

END
 
  Today, there have been 16 visitors (52 hits) on this page! Copyright 2007  
 
=> Do you also want a homepage for free? Then click here! <=
Welcome visitor
free counters