'{$STAMP BS2p} ' Subsume_Libby1.BSP - This shows the Subsumption engine in the Stamp 2p40. ' It uses the ramping servo controllers and sensors from the Co-Processor ' Copyright 2002, 2003 Blue Bell Design Inc. ' Changed sense of direction bits to match Co-Processor 8/7/03 ' This code is shown to be instructive. You don't need to run it as an ' equivalent is already IN the Co-Processor. ' NOTE: Some of this code is derived from the code for the TRaCY robot. ' TRaCY code is Copyright Dennis Clark and Harry W. Lewis 1999 and 2000 ' -----[ 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 1021 ' 2400 BAUD => BS2 = 396, BS2p = 1021 = CoProc V1.0 'BAUD CON 240 ' 9600 BAUD => BS2 = 84, BS2p = 240 ' CoProc V1.1 pin 21 pulled up = 2400 Baud (jumper out) ' CoProc V1.1 pin 21 grounded = 9600 Baud (jumper in) 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 backward 'xx = 10 -> left motor forward 'xx = 11 -> left motor forward 'yy = right motor 'normal list follows fd CON %1010 'forward = 10 rv CON %0101 'reverse = 5 st CON %0000 'stop = 0 tr CON %1000 'turn right = 8 tl CON %0010 'turn left = 2 rr CON %1001 'rotate right = 9 rl CON %0110 'rotate left = 6 bl CON %0100 'backup turning left = 4 ' -----[ 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 - This is automatic in the Co-Processor. 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.Bit3 = 1 THEN lftfwd 'jump if want to move left motor forward IF drive.Bit2 = 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.Bit1 = 1 THEN rtfwd 'see comments for left side above IF drive.Bit0 = 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