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
