'>Hextor24.BSx '============================================================== 'Milford Instruments http://www.milinst.com 'Hextor Design, Bug Commander - Behavioural Operating System 'and the Behavioural Controller programs by David Buckley '================================================================ '{$stamp BS2Sx} '============================================================== 'Dosequence modified to allow h,v,w commands. 'Two voice sequences added in initialisation at init1. 'The first, a chirp, is used instead of a pause whilst waiting 'for the infra-red circuitry to settle and the second, a beep, 'to indicate ready. ' 'New '--- ' 'Required knowledge: ' 'Meta command: 'BOS Commands: V, h, v, w 'Common RAM: 'Constants: 'Variables: 'Routine: 'Subroutine: dosequence modified 'Sequence: chirp1, beep2 'usRules: 'Behaviour: '============================================================== 'Experiment with sounds and gripper position control '============================================================== 'Direct Control 'turns on servo power and exits with servo power on 'BOSbusy goes low immediately, doesn't wait for move to finish '$speed;$servo - high nibble and low nibble respectively ' BOScmnd,arg1.arg2 ' 0,$speed;$servo,position ' 1,$XX,$servo,X ->P actual servo position ' 2,$XX,$servo,X ->S servo status, bit1-on bit0-done ' 3,X,X ->hiF,then loF two bytes all servo moved status ' 4,X,X ->hiR,then loR two bytes all servo on/off status ' 5,$XX;$servo,X stop pulsing servo ' '-------- Legs & Logical-Servos -------- ' Plan view of TecBug S - Servo(0 - 13) L - Legs(1 - 6) ' S L L S ' ( ) gripper 13 ' | | lift 12 ' L---F---R LEG NUMBERS ' 1,0 | 1 2 | 6,7 1=LF 2=RF ,L - left, R - Right ' 3,2 | 3 4 | 8,9 3=LM 4=RM ,M - mid, F - Front ' 5,4 | 5 6 | 10,11 5=LB 6=RB ,B - Back ' \-------/ ' 'Servo position ' f - forward m - middle b - backward 'Left Lf=255 Lm=127 Lb=0 'Right Rf=0 Rm=127 Rb=255 ' ' d - Down u - Up 'Left Ld=0 Lu=255 'Right Rd=255 Ru=0 '-------------------------------------------------------------- 'Sequences in DATA statments '--------------------------- 'Meta commands - '"a",word jump to sequence 'name' '"i",word includes sequence 'name' as a subroutine ' chirp1 DATA "v",1,20,"zv",100,30,"v",2,20,"z" beep2 DATA "Vh",1,"Vz" wiggle1 DATA "UDFBCRLOBPMz" wiggle2 DATA "UFFBBz" wiggle3 DATA "UFRRLLBz" setting1 DATA "Qs",4,"r",170,"z" setting2 DATA "Ts",0,"r",200,"z" 'feet drag if not "T" tround DATA "s",15,"RRRRRRRRRRRRs",4,"z" beg DATA "s",4,"U" 'speed 4, Up DATA 0,$43,127 'speed 4, servo 3, leg 3 up DATA 0,$49,127 'speed 4, servo 9, leg 4 up DATA 0,$42,170 'speed 4, servo 2, leg 3 forward DATA 0,$48,90 'speed 4, servo 8, leg 4 forward DATA "WU" 'wait for move to finish then Up DATA 0,$25,255 'speed 2, servo 5, leg 5 up DATA 0,$2B,0 'speed 2, servo 11, leg 6 up DATA 0,$F1,255 'speed 15, servo 1, leg 1 up DATA 0,$F7,0 'speed 15, servo 7, leg 2 up DATA 0,$40,200 'speed 4, servo 0, leg 1 forward DATA 0,$46,50 'speed 4, servo 6, leg 2 forward DATA "s",2,"WUz" 'speed 2, wait, Up, end of list Rface DATA "s",15,"RRR","s",2,"z" Lface DATA "s",15,"LLL","s",2,"z" F5 DATA "s",15,"FFFFF","s",2,"z" R5 DATA "s",15,"RRRRR","s",2,"z" L5 DATA "s",15,"LLLLL","s",2,"z" BR DATA "BRz" BL DATA "BLz" 'Ultrasonic sensor '----------------- 'Meta commands - '"a" if selected by the test then triggers the Action sequence ' The first parameter byte for "Y" or "y" is the index to ' the yawn variable, 0=>usyawn0, 1=>usyawn1. ' The second parameter byte is the threshold '"Y" if selected by the test increments 'usyawnX'. ' If 'usyawnX'> threshold then skip the following rule. '"y" if selected then resets 'usyawnX' to 0 'so select same parameters as rule to be skipped for both Y, y ' 'Rules - 'DATA direction,range,action if >,action if <,sequence pointer 'If the rule doesn't contain "a" then no sequence can be 'activated by it so any value could be used for the pointer 'value. uscorner DATA "S" 'default action DATA usF, 20,"XB",Word 0 'back if <=20 DATA usR2, 20,"XL",Word 0 'left if <=20 DATA usL2, 20,"XR",Word 0 'right if <=20 DATA usF, 25,"Xa",Word tround 'Action if <=25 DATA usR2, 25,"Xa",Word tround 'Action if <=25 DATA usL2, 25,"Xa",Word tround 'Action if <=25 DATA usF, 80,"XF",Word 0 'forward if <80 DATA usR2, 80,"XR",Word 0 'right if <80 DATA usL2, 80,"XL",Word 0 'left if <80 DATA 0 'end of list usbeg DATA "S" DATA usF, 45,"Xa",Word beg 'Action if <=45 DATA usR2, 35,"Xa",Word beg 'Action if <=35 DATA usL2, 35,"Xa",Word beg 'Action if <=35 DATA 0 'end of list 'because the parameters for the four rules are the same if beg 'was going to be activated then "Y" would be selected and beg 'activated until yawn=4 then the beg rule would be skipped and '"y" and then tround activated. ushello DATA "S" DATA USF, 20,"XB",Word 0 'back if <=20 DATA USR2, 20,"Xa",Word BL 'left back if <=20 DATA USL2, 20,"Xa",Word BR 'right back if <=20 DATA USF, 45,"XY",0,3 'increment usyawn0 'IF usyawn0>3 then skip 'IE do beg 3 times before skipping DATA USF, 45,"Xa",Word beg 'Action if <=45 DATA USF, 45,"Xy",0,0 'reset usyawn0 if <=45 DATA usF, 45,"Xa",Word tround 'Action if <=45 DATA USR2, 45,"Xa",Word Rface 'Action if <=45 DATA USL2, 45,"Xa",Word Lface 'Action if <=45 DATA USF, 100,"Xa",Word F5 'Action if <=100 DATA USR2,100,"Xa",Word R5 'Action if <=100 DATA USL2,100,"Xa",Word L5 'Action if <=100 DATA 0 'end of list '-------------------------------------------------------------- 'common RAM '---------- 'Note there is no test for the stack being too big, beware of 'the stack growing down through 0! '61 down sequence stack '62 sequence stack pointer '-------------------------------------------------------------- BOStx CON 0 'pin serial to BOS processor BOSrx CON 1 'pIN SERIal from BOS processor epower CON 2 'piN HIGH Turns on power to electronics BOShalt CON 3 'pin high halts current BOS move BOSbusy VAR IN4 'PIN BOS processor busy => high irrx CON 5 'PIN INFRa red comms in, i96n usrx CON 6 'pin ultrasonic commands, i96n ustx CON 7 'pin ultrasonic data in, i96n Rled CON 8 'high turns on right green led Lled CON 9 'high turns on left red led poweruptime CON 1500 'pause before powering up electronics ustout CON 10000 'timeout min 6000 for 0 to 255 travel usmin CON 10 'right,increase if servo hits end stop usahead CON 127 'alter so servo points straight ahead usmax CON 255 'left, decrease if servo hits end stop i96n CON 16624 '9600 baud, 8 bit, no parity, inverted 'us servo DIreCtions, 0 is a reserved direction for end of list usR3 CON 27 'us servo direction Right a lot usR2 CON 77 'us servo direction Right middle usR1 CON 107 'us servo direction Right a little usF CON 127 'us servo direction Forward usL1 CON 147 'us servo direction Left a little usL2 CON 177 'us servo direction Left middle usL3 CON 227 'us servo direction Left a lot BOScmnd VAR Byte 'byte to send to BOS processor Sptr VAR Word 'pointer to DATA sequence string Sptrl VAR Sptr.LOWBYTE Sptrh VAR Sptr.HIGHBYTE arG VAR Word arG1 VAR arg.LOWBYTE 'command argument arG2 VAR arg.HIGHBYTE 'command argument irin VAR Byte 'infra-red command oldirin VAR Byte 'old infra-red command usrules VAR Word 'pointer to current rules usdir VAR Byte 'us servo direction, 255=left 0=right usrange VAR Byte 'us servo range, 15-255 usrptr VAR Word 'pointer to DATA us Rules usrrange VAR Byte 'range to trigger action usactiong VAR Byte 'action byte if > triggered usactionl VAR Byte 'action byte if <= triggered usyawn0 VAR Nib 'counter0 for how bored robot is usyawn1 VAR Nib 'counter1 for how bored robot is incS VAR Nib 'number of sequences included temp VAR Word stkp VAR Byte 'point to top of CommonRam stack 'during downloading the power to the electronics is turned off, 'iF it is turned back on again too soon the electronics, 'especially the ultrasonics do not power up properly, 'sO wait poweruptime before turning power on. init: OUTPUT Rled OUTPUT LleD HIGH Rled HIGH Lled PAUSE poweRUPTime HIGH epoweR 'turn on electronics inIT1: GOSUB IRGEt SPtr =chirP1 GOSUB doseQUENce IF IRIn<>0 THEN init1 'wait for ir to initialise DEBUG CLS PUT 62,62 'initialise stack pointer Sptr =beep2 GOSUB dosequence 'Program start '------------- start: '---- Main Behaviour Controller - Infra-Red ---- irCON: TOGGLE Rled PAUSE 50 GOSUB irget IF irin=0 THEN start '0 is idle value IF irin=oldirin THEN start OLDIRin =irin 'irin changed IF irin=1 THEN wig1 'trained key 1 IF Irin=2 THEN dobeg 'trained key 2 IF irin=3 THEN wig3 'trained key 3 IF IRin=4 THEN set1 'trained key 4 IF irin=5 THEN set2 'trained key 5 IF Irin=6 THEN douscorner 'trained key 6 IF irin=7 THEN doushello 'trained key 7 IF IRin=8 THEN BOScon 'trained key 8 TOGGLE LleD PAUSE 50 GOTO start wig1: Sptr =wiggle1 GOSUB doseqUENCe GOTO start 'check ir wig2: Sptr =wiggle2 GOSUB dosequence GOTO start 'check ir wig3: Sptr =wiggle3 GOSUB dosequence GOTO start 'check ir set1: Sptr =setting1 GOSUB dosequence GOTO start 'check ir set2: Sptr =setting2 GOSUB dosequence GOTO start 'check ir dobeg: Sptr =beg GOSUB dosequence GOTO start 'check ir '---- Behaviour Controller - Ultrasonic Sensor ---- douscorner: usrules =uscorner GOSUB irget 'check ir IF irin=6 THEN douscorner 'wait for idle signal GOTO usbehave dousbeg: usrules =usbeg GOSUB irget 'check ir IF irin=7 THEN dousbeg 'wait for idle signal GOTO usbehave doushello: usrules =ushello GOSUB irget 'check ir IF irin=7 THEN doushello 'wait for idle signal 'and drop through to usbehave usBEHAve: GOSUB irget 'check ir IF Irin<>0 THEN start 'other ir command received usdo: UsRptr =usruleS READ usRptr,BOScmnd 'read default USRPtr =usRptr +1 ustest: READ usRptr,usdir IF usdir=0 THEN usmove 'end of list, do default usRptr =usRptr +1 READ usRptr,usRrange usRptr =usRptr +1 READ usRptr,usactiong usRptr =usRptr +1 READ usRptr,usactionl usRptr =usRptr +1 READ usRptr,Sptrl 'read pointer to sequence usRptr =usRptr +1 READ usRptr,Sptrh 'read pointer to sequence usRptr =usRptr +1 'dEBUG dec usdir,dec usRrange,usactiong,usactionl,cr GOSUB usgeT IF usRange>USRRange THEN usg usl: IF usactionl="X" THEN ustest 'ignore it BOScmnd =usactionl 'action triggered GOTO usmove 'do it usG: IF usactiong="X" THEN ustest 'ignore it BOScmnd =usaCTiong 'action triggered usmove: IF BOScmnd<>"Y" THEN usy BRANCH Sptrl,[usincY0,usincY1] GOTO ustest 'ignore if usactionl is too big usincY0: usyawn0 =usyawn0 +1 IF usyawn0<=Sptrh THEN ustest usRptr =usRptr +6 'skip next rule GOTO ustest usincY1: usyawn1 =usyawn1 +1 IF usyawn1<=Sptrh THEN ustest usRptr =usRptr +6 'skip next rule GOTO ustest usy: IF BOScmnd<>"y" THEN usm BRANCH Sptrl,[uszeroY0,uszeroY1] GOTO ustest 'ignore if usactionl is too big uszeroY0: usyawn0 =0 GOTO ustest uszeroY1: usyawn1 =0 GOTO ustest usm: IF BOScmnd="a" THEN sact GOSUB tellBOS 'now do action GOTO usbehave 'do it sact: GOSUB dosequence GOTO usbehave '---- Behaviour Controller - Direct to BOS ---- ' turns on servo power and exits with servo power on ' $speed;$servo - high nibble and low nibble respectively ' 0,$speed;$servo,position ' 1,$XX,$servo,X ->P actual servo position ' 2,$XX,$servo,X ->S servo status, bit1-on bit0-done ' 3,X,X ->hiF,then loF two bytes all servo moved status ' 4,X,X ->hiR,then loR two bytes all servo on/off status ' 5,$XX;$servo,X stop pulsing servo BOScon: GOSUB irget 'check ir IF irin=8 THEN BOSdo 'for us IF irin<>0 THEN start 'other ir command received BOSdo: BOScmnd =0 arg1 =$90 arg2 =200 GOSUB tellBOS3 'move 0 FOR arg2 =0 TO 9 'use arg2 as a temp variable BOSCMnd =1 ARG1 =0 GOSUB tellBOS3 'ask position GOSUB BOSreply1 DEBUG"pulsing", BIN8 BOScmnd,BIN8 arg1,CR PAUSE 20 NEXT BOScmnd =4 GOSUB tellBOS3 'ask pulsing GOSUB BOSreply2 DEBUG"pulsing", BIN8 BOScmnd,BIN8 arg1,CR BOScmnd =5 arg1 =0 GOSUB tellBOS3 'stop pulsing PAUSE 1000 BOScmnd =4 GOSUB tellBOS3 'ask pulsing GOSUB BOSreply2 DEBUG"pulsing", BIN8 BOScmnd,BIN8 arg1,CR BOScmnd =0 arg1 =$30 arg2 =50 GOSUB tellBOS3 'move 0 PAUSE 150 BOScmnd =3 GOSUB tellBOS3 'ask finished GOSUB BOSreply2 DEBUG"finished", BIN8 BOScmnd,BIN8 arg1,CR PAUSE 2000 BOScmnd =3 GOSUB tellBOS3 'ask finished GOSUB BOSreply2 DEBUG"finished", BIN8 BOScmnd,BIN8 arg1,CR GOTO BOScon '------------- subroutines --------------------- push: 'stacks the single byte arg1 GET 62,stkp '62 is stack pointer stkp =stkp -1 'next location PUT stkp,arg1 'push byte PUT 62,stkp 'save pointer RETURN '------------------------------- pop: 'unstacks a single byte into arg1 GET 62,stkp '62 is stack pointer stkp =stkp+1 MAX 62 'previous location GET stkp,arg1 'pop byte PUT 62,stkp 'save pointer RETURN '------------------------------- pushword: 'stacks the word arg GET 62,stkp '62 is stack pointer stkp =stkp -1 'next location PUT stkp,arg1 'push byte stkp =stkp -1 'next location PUT stkp,arg2 'push byte PUT 62,stkp 'save pointer RETURN '------------------------------- popword: 'unstacks the word arg GET 62,stkp '62 is stack pointer stkp =stkp+1 MAX 62 'previous location GET stkp,arg2 'pop byte stkp =stkp+1 MAX 62 'previous location GET stkp,arg1 'pop byte PUT 62,stkp 'save pointer RETURN '------------------------------- usget: 'talk to UltraSonic co-processor usRange =0 'invalid value, used if no repyl usdir =usdir MIN usmin MAX usmax 'don't hit end stops usout: SEROUT ustx,i96n,["T",usdir] SERIN usrx,i96n,ustout,us1,[usRange] 'jump if no reply DEBUG DEC usdir," ",DEC usRange," ",CR RETURN us1: DEBUG "no reply from ultrasonics",CR RETURN '-------------------- irget: 'listen to InfraRed co-processor irin =15 'set to invalid value SERIN irrx,i96n,100,noir,[irin] 'valid 0 - 8 irin =irin & $F 'mask high nibble timing data noir: RETURN '-------------------- dosequence: 'loop round until BCbyte="z" READ Sptr,BOScmnd IF BOScmnd="z" THEN doneS Sptr =Sptr+1 'point to command IF BOScmnd="a" THEN Sgo IF BOScmnd="i" THEN Sinc IF BOScmnd<6 THEN Scon arg1 =255 LOOKDOWN BOScmnd,["gv"],arg1 IF arg1=255 THEN t2 'BOScmnd not gv, jump to t2 GOSUB readword GOSUB tellBOS2d GOTO dosequence t2: LOOKDOWN BOScmnd,["srmlhw"],arg1 IF arg1=255 THEN t1 'BOScmnd not srmlhvw, jump to t1 READ Sptr,arg1 'read it Sptr =Sptr +1 GOSUB tellBOS1d 'send them GOTO dosequence t1: GOSUB tellBOS GOTO dosequence Scon: GOSUB readword GOSUB tellBOS3 'wait until not busy then send 3 bytes IF BOScmnd=0 THEN dosequence IF BOScmnd>2 THEN rep2 rep1: GOSUB BOSreply1 'reply not used GOTO dosequence rep2: GOSUB BOSreply2 'reply not used GOTO dosequence Sgo: GOSUB readword 'read new Sptr Sptr =arg 'make Stpr=new Sptr GOTO dosequence 'do new sequence Sinc: incS =incS +1 'update includes level GOSUB readword 'read new Sptr temp =arg 'keep it safe arg =Sptr 'save old Sptr GOSUB pushword Sptr =temp 'make Stpr=new Sptr GOTO dosequence 'do included sequence doneS: IF incS=0 THEN retS GOSUB popword 'get where we were before include Sptr =arg incS =incS MIN 1 -1 'update includes level GOTO dosequence retS: RETURN '-------------------- readword: READ Sptr,arg1 Sptr =Sptr +1 READ Sptr,arg2 Sptr =Sptr +1 RETURN '-------------------- tellBOS: PAUSE 10 'give the BOS time to go into receive mode IF BOSbusy=1 THEN tellBOS 'wait until BOS not busy SEROUT BOStx,i96n,[BOScmnd] RETURN '-------------------- tellBOS1d: 'send command plus one argument in decimal PAUSE 10 'give the BOS time to go into receive mode IF BOSbusy=1 THEN tellBOS1d 'wait until BOS not busy SEROUT BOStx,i96n,50,[BOScmnd,DEC arg1,CR] '50 pace time RETURN '-------------------- tellBOS2d: 'send command plus two arguments in decimal PAUSE 10 'give the BOS time to go into receive mode IF BOSbusy=1 THEN tellBOS2d 'wait until BOS not busy SEROUT BOStx,i96n,50,[BOScmnd,DEC arg1,CR,DEC arg2,CR] '50 pace time RETURN '-------------------- tellBOS3: IF BOSbusy=1 THEN tellBOS3 SEROUT BOStx,i96n,1,[BOScmnd,arg1,arg2] 'pace min 1 RETURN '-------------------- BOSreply1: 'wait min 40 SERIN BOSrx,i96n,50,BOSr1,[arg1] 'get byte BOSr1: RETURN '-------------------- BOSreply2: 'wait min 210 'get highbyte,lowbyte of flags SERIN BOSrx,i96n,250,BOSr2,[arg1,arg2] BOSr2: RETURN '------------------------ end program -------------------------