'Combot revision 4 Critter revision 'Combat robot for MileHiCon Critter Crunch by DLC 10/6/99 'Fully subsumptive architecture using FSMs for better response times ' 'What is a subsumtion architecture? In a nutshell it is a collection 'of behaviours that are totally independent, have zero coupling to 'each other and do not share state. In an ideal world, all of the 'behaviours would be running simultaneously. However, the Stamp II 'does not support multitasking, so rather than have any one function 'or behaviour monopolize the processor for its entire process I have 'made all behaviours Finite State Machines (FSM for short). This 'allows all of these rather simple behaviours to get a "slice" of 'time and release the processor for the next one in line in such a 'manner that things happen rather quickly in order, that closely 'resembles concurrency. I do this by having each behaviour save 'its current state so that it can come back to it and act on its 'time slice. I have located the routines in the main loop in such 'a manner as to allow a heirarchy of priority based on position. 'Namely, the behaviours closest to the "act" function will have 'priority over those further up the line. This isn't that difficult 'to do, check this code over if you don't believe it. Notice that 'having zero coupling also makes the programming much simpler, and 'allows for interesting behavioural patterns to show through that 'have NOT been pre-programmed! 'General constants and variables SPKR con 1 'speaker I/O port LED con 0 'LED flasher LCD con 10 'LCD option IRPD con 15 'IRPD enable pin i var byte 'temporary variable detect var byte 'IRPD holder seed var word 'random word seed ustate var seed.byte1 'uberstate, behaviour path utask var ustate.highnib 'actual state usub var ustate.lownib 'subtask within that path lseen var seed.byte0 'the direction target last seen 'servo constants and variables SC con 7 'I/O port of TTT servo chip N2400 con 1021 '8N1 2400 non-inverted serial N9600 con 240 'for LCD etc. drive var word 'motor variable, both wheels ldrive var drive.byte1 'left drive parts rdrive var drive.byte0 SRIGHT con 64 'servo 1 on TTT servo chip SLEFT con 0 'servo 0 on TTT servo chip servo var byte 'general servo value holder 'Wheel movement routine variables lright var byte 'last right wheel value used lleft var byte 'last left... 'Motor movement values [left|right] f con $3f01 'forward bu con $013f 'reverse st con $1c1b 'stop (mostly) tl con $1c01 'turn left fl con $1f05 'turn left going forward tr con $3f1b 'turn right fr con $3a17 'turn right going forward rl con $1010 'rotate left rr con $2f2f 'rotate right 'Seek FSM constants and vars sDur var word 'duration, times through FSM sDir var word 'seek direction 'Chase FSM constants and vars 'detect = in3*2 + in2 'Bumper vars and constants SKIRT con 12 'bumper skirt option bDur var word 'duration, times through FSM bDir var word 'actual direction to go 'Attack vars and constants LIFT con 192 'servo 3 on TTT servo chip goup con 01 'go up godown con 43 'go down WSKR con 9 'sensor to trigger lift atDur var word 'duration, times through FSM atDir var byte '0 = down, 1=up, 2=locked 'i = in9 for whisker status 'watchEdge vars and constants RCT con 8 'edge detect sensor I/O port weDur var word 'duration, times through FSM weDir var word 'direction to go 'abort FSM vars and constants ABORT con 11 'potential abort RC command 'uses i = in11 for value Startup: 'initialize variables, servos, and sensors sDur =0 atDur =0 bDur =0 weDur =0 lseen = 0 'nothing last seen high SC 'make sure serout is high high IRPD 'enable IRPD gosub stopWheels 'turn off drive wheels gosub setlift 'reset lifter now gosub setupmotors 'get motors ready gosub startWheels 'and enable wheels freqout SPKR, 500,2000 'announce "I'm alive and ready" freqout SPKR, 1000,1000 pause 500 'wait for a half second gosub movetocenter 'move to center of board and begin seek main: 'This is the main loop gosub seek 'wander mode ' gosub bumper 'subsumes seek (not done) gosub chase 'subsumes seek and bumper gosub attack 'subsumes seek,bumper,chase gosub watchEdge 'subsumes seek,bumper,chase,attack ' gosub eabort 'subsumes all functions (not done) gosub act 'do what won above goto main end 'need this to keep Stamp in line! '=============================== '= Robotic Behaviours = '=============================== seek: 'wander, seeking enemy if sDur <> 0 then seDec 'do this if sDur is 0 random seed 'random direction i = seed & %111 'mask off for 0-7 only lookup i,[f,tr,tl,f,rr,f,fr],sDir random seed 'random direction and duration sDur = seed & %1111111 'mask for 128 choices sDur = sDur + 50 'a bit more duration seDec: 'sDur <>0, so do something sDur = sDur - 1 'we have been here drive = sDir return chase: 'enemy found, go get it! detect = in3*2 + in2 'look at IRPD lseen = detect 'where last seen branch detect,[cdone,cleft,cright,cfront] cfront: drive = f goto cdone cleft: drive = tl goto cdone cright: drive = tr cdone: return bumper: 'bumped them, rotate and seek if bDur > 0 then bDec 'only act if if in12 <> 0 then bdone 'only act if bumper = 0 (hit) bDir = rl bDur = 500 bDec: 'we are acting on a bumper hit bDur = bDur - 1 'we have acted drive = bDir bDone: return watchEdge: 'look for "end of the world" if weDur > 0 then weDec 'only do this if we aren't already if in8 = 1 then weOK weDur = 500 'times through this FSM weDir = bu 'backup high 0 weDec: if weDur > 250 then weDec2 low 0 weDir = rr 'rotate away weDec2: weDur = weDur - 1 drive = weDir weOK: return eabort: 'look for radio abort i = in11 'get status of radio abort if i <> 0 then dOK 'if so, then do edge hit weDur = 500 'times through this FSM weDir = bu 'backup high 0 dOK: return act: 'This runs the servo wheels servo = SLEFT + ldrive 'get left motor value if servo = lleft then doRight serout SC,N2400,[servo] 'only do this if different lleft = servo 'new last value doRight: servo = SRIGHT + rdrive if servo = lright then aDone serout SC,N2400,[servo] 'only do this if different lright = servo 'new last value aDone: return attack: 'picked one up, now lift him if atDur > 0 then atDec 'go to decrement section if in9 = 1 then atDone 'nothing there, do nothing now drive = f 'RUN TO THE EDGE!! if atDir = 1 then atDec 'already up gosub liftUP 'got one! take up the lift atDur = 280 'er, do this for a while atDir = 1 'its up atDec: 'look for the capture and decrement if in9 = 0 then atDec2 'go to true decrement if atDir = 0 then atDec2 'already down, just decrement gosub liftDOWN 'lost the target atDur = 280 atDir = 0 'its going down atDec2: 'just decrement now atDur = atDur - 1 if atDir <> 1 then atDec3 'no victim drive = f atDec3: 'when really ready if atDur > 0 then atDone gosub setlift 'center switch to limit air loss atDone: if in8 = 1 then atSafe 'not on the edge if atDir <>1 then atSafe'only when loaded do we stop gosub stopwheels 'on the edge! gosub liftDOWN pause 1500 gosub setlift freqout SPKR,300,2000 freqout SPKR,500,1000 freqout SPKR,500,1500 freqout SPKR,1000,2000 freqout SPKR,750,1000 freqout SPKR,500,2000 stop atSafe: 'safe here return '-------------------------------- '- Routines used above - '-------------------------------- stopwheels: 'stops wheels from moving serout SC,N2400,[00] return startwheels: 're-starts wheels serout SC,N2400,[$40] return liftUP: 'up with the lift servo = goup + LIFT serout SC,N2400,[servo] return liftDOWN: 'drop the lift servo = godown + LIFT serout SC,N2400,[servo] return setlift: 'centers lifter switch servo = 25 + LIFT serout SC,N2400,[servo] return setupmotors: drive = st i = ldrive + SLEFT 'set motors to stop and enable them serout SC,N2400,[i] i = rdrive + SRIGHT serout SC,N2400,[i] return movetocenter: 'start by getting away from edge sDir = f 'go forward sDur = 100 ' several feet return