'>Hxtr2-2.BSx '============================================================== 'Milford Instruments http://www.milinst.com 'Hextor Design, Bug Commander - Behavioural Operating System 'and the Behavioural Controller programs by David Buckley '================================================================ '{$stamp BS2Sx} DEBUG "prog2" '============================================================== 'Hxtr2-0.BSx - main 'Hxtr2-1.BSx - Activity sequencer 'Hxtr2-2.BSx - US Behaviour 'Hxtr2-3.BSx - Direct Control 'Hxtr2-4.BSx - Prog4 'Hxtr2-5.BSx - Prog5 'Hxtr2-6.BSx - Prog6 'Hxtr2-7.BSx - Utils- LCD menu and IR remote 'Hxtr2-Doc.BSx - Meta Commands and Program Headers '============================================================== 'New '--- ' 'Required knowledge: ' 'Meta command: 'BOS Commands: 'Common RAM: 'Constants: 'Variables: 'Routine: 'Subroutine: 'Labels: ' 'usRules: 'Behaviour: '============================================================== 'History '------- '15Aug05 FokLEDs,Fokfeelers,Fokus renamed FuseLEDs,Fusefeelers,FuseUS '============================================================== 'Experiment with '============================================================== '-------- Legs & Logical-Servos -------- ' ' Plan view of Hextor 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 values and leg positions ------- ' 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 ' '-------- Arm and Gripper positions ------- 'Arm up 255 'Arm down 0 'Gripper open 255 'Gripper closed 0 '-------------------------------------------------------------- 'Sequencer program1 '------------------ A_null CON 0 A_chirp1 CON 1 A_beep2 CON 2 A_wiggle1 CON 3 A_wiggle2 CON 4 A_wiggle3 CON 5 A_Qs4r170 CON 6 A_Ts0r200 CON 7 A_tround CON 8 A_beg CON 9 A_Rface CON 10 A_Lface CON 11 A_F5 CON 12 A_R5 CON 13 A_L5 CON 14 A_BR CON 15 A_BL CON 16 A_L3 CON 17 A_R3 CON 18 A_L2 CON 19 A_R2 CON 20 A_Iflash CON 21 A_Fbump CON 22 A_FbumpRI1 CON 23 A_FbumpLI2 CON 24 A_FbumpBI3 CON 25 A_last CON 25 'last routine ' 'Ultrasonic-behaviour name-table program2 '---------------------------------------- 'name table to copy into other programs '-------------------------------------- US_null CON 0 US_sitby CON 1 US_beg CON 2 US_hello CON 3 US_wander CON 4 US_user1 CON 5 US_user2 CON 6 US_user3 CON 7 US_user4 CON 8 US_last CON 8 'last routines 'Data pointers CON*2 because Rule pointers are words '--------------------------------------------------- DATA @US_sitby *2,Word sitby DATA @US_beg *2,Word beg DATA @US_hello *2,Word hello DATA @US_wander *2,Word wander DATA @US_user1 *2,Word null 'replace with your own DATA @US_user2 *2,Word null 'replace with your own DATA @US_user3 *2,Word null 'replace with your own DATA @US_user4 *2,Word null 'replace with your own 'Meta commands - '"I" if selected by the test then includes a Prog/Option, ' first parameter is prognumber, second is Option. ' Return address is stacked. ' "I" can be used to run any program + option including ' US behaviours, however yawn variables values will be ' inherited. '"X" if action ="X" then ignore that result. '"J" if action ="J" then jump to new program, no stack action ' first parameter is prognumber, second is option ' so we can even run another US behaviour. ' If the Option is 0 then the jumped to Prog will be forced ' to check the stack and so acts as RETURN. 'Yawn '---- ' The yawn rules are special in that when an outcome of ' other rules is activated scanning through the rule set is ' terminated, however the "Y" rule only tests whether the ' next rule should be used, and the "y" rule only resets ' the "yawn" variable before the rule following it is used. ' 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 for rule to be skipped for both Y, y ' '-------------- ' 'Rules - 'DATA direction,range,do if >,do if <,activity program,activity 'If the rule doesn't contain "I" then no sequence can be 'activated by it so any value can be used for the pointer values. null DATA "V",0 'null routine just beep sitby DATA "S" 'default action DATA usF, 20,"XB",0,0 'back if <=20 DATA usR2, 20,"XL",0,0 'left if <=20 DATA usL2, 20,"XR",0,0 'right if <=20 DATA usF, 25,"XI",1,A_tround 'action if <=25 DATA usR2, 25,"XI",1,A_tround 'action if <=25 DATA usL2, 25,"XI",1,A_tround 'action if <=25 DATA usF, 80,"XI",1,A_F5 'F5 if <80 DATA usR2, 80,"XI",1,A_R5 'R5 if <80 DATA usL2, 80,"XI",1,A_L5 'L5 if <80 DATA 0 'end of list beg DATA "S" DATA usF, 45,"XI",1,A_beg 'action if <=45 DATA usR2, 35,"XI",1,A_beg 'action if <=35 DATA usL2, 35,"XI",1,A_beg 'action if <=35 DATA 0 'end of list hello DATA "S" DATA usF, 20,"XB",0,0 'back if <=20 DATA usR2, 20,"XI",1,A_BL 'left back if <=20 DATA usL2, 20,"XI",1,A_BR 'right back if <=20 ' Because the parameters for the next four rules are ' the same, if beg was going to be activated by the ' second rule then "Y" would be selected by the first ' rule and the Yawn variable incremented. When yawn=4 ' then the beg rule would be skipped and the third ' rule would activate "y" reseting the Yawn variable. ' Then the fourth rule would activate tround. DATA usF, 45,"XY",0,3 'increment usyawn0 'if usyawn0>3 then skip 'ie do beg 3 times before skipping DATA usF, 45,"XI",1,A_beg 'action if <=45 DATA usF, 45,"Xy",0,0 'reset usyawn0 if <=45 DATA usF, 45,"XI",1,A_tround'action if <=45 DATA usR2, 45,"XI",1,A_Rface 'action if <=45 DATA usL2, 45,"XI",1,A_Lface 'action if <=45 DATA usF, 100,"XI",1,A_F5 'action if <=100 DATA usR2,100,"XI",1,A_R5 'action if <=100 DATA usL2,100,"XI",1,A_L5 'action if <=100 DATA 0 'end of list wander DATA "F" DATA usF, 40,"XI",1,A_tround'action if <=30 DATA usR1, 40,"XI",1,A_L3 'left 2 if <=30 DATA usR2, 30,"XI",1,A_L2 'left 3 if <=30 DATA usL2, 30,"XI",1,A_R2 'right if <=30 DATA usL1, 40,"XI",1,A_R3 'right if <=30 DATA 0 'end of list '-------------------------------------------------------------- 'common RAM '0 program to return to - all '1 Program option - Activity to do, USrules to do, Options '2 current Utility routine '3 usyawn0 - US-behaviour '4 usyawn1 - US-behaviour '5 irin - current IR value on change of pgm '6 oldbtn - current button on change of pgm '7 menc - last menu code '8 mopts - menu option OAUIP '9 flags - various flag bits ' bit0=IRwatch - 0=>ignore IR signal, 1=>act on it ' bit1=useLEDs - 0=>ignore LED cmnds, 1=>enable LEDs ' (If pins remapped) ' bit2=usefeelers - 0=>ignore feelers ' bit3=useus - 0=>don't run US routines ' bit4=warmIR - 1=>IR chip needs no warmup time ' bit7=Flagsvalid - 0 after download, 1->ok 'Note there is no test for the stack being too big, beware of 'the stack growing down through 9! '60 down Sequence Stack '61 sequence Stack pointer '62 letterbox for push and pop '63 active program number used by BS2Sx '-------------------------------------------------------------- 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 lcdtx CON 6 'pin to send to LCD lcdrx CON 7 'pin to receive from LCD ustx CON 14 'pin ultrasonic commands, i96n usrx CON 15 'pin ultrasonic data in, i96n 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 tempb VAR Byte retprog VAR tempb 'program to return to doprog VAR Byte 'Program to do yindx VAR doprog 'index to yawn var stkp VAR Byte 'point to top of CommonRam stack irentry VAR Byte 'irin on entry to pgm Activity VAR Byte 'Activity to do ythresh VAR Activity 'yawn threshold arg VAR Word arg1 VAR arg.LOWBYTE 'command argument arg2 VAR arg.HIGHBYTE 'command argument argl VAR arg.LOWBYTE argh VAR arg.HIGHBYTE Prog VAR argh.HIGHNIB irin VAR Byte 'infra-red command usname VAR Byte 'current rule name usrules VAR Word 'pointer to current rules Dptr VAR usrules 'used in readword ustout CON 10000 'timeout min 6000 for 0 to 255 travel 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 'us Rule range to trigger action usactiong VAR Byte 'action byte if > triggered usactionl VAR Byte 'action byte if <= triggered usyawn VAR Byte usyawn0 VAR usyawn.LOWNIB 'boredom counter0 usyawn1 VAR usyawn.HIGHNIB 'boredom counter1 lcdbtns VAR Byte 'button states are in lower nibble btn1state VAR lcdbtns.BIT0 btn2state VAR lcdbtns.BIT1 btn3state VAR lcdbtns.BIT2 btn4state VAR lcdbtns.BIT3 btn VAR lcdbtns.HIGHNIB '0 - 4 current button state flags VAR Byte 'various flags Firwatch VAR flags.BIT0 '1=>watch for IR FuseUS VAR flags.BIT3 'future prog2 init: HIGH epower 'keep electronics on GET 1,usname 'read doUSrule PUT 1,0 'so we don't do it again and ... IF usname<>US_null THEN dousname lookstack: GET 61,stkp IF stkp=61 THEN RUN0 'nothing else to do GOSUB pop_arg 'no name so check stack IF Prog=2 THEN gotname GOSUB push_arg 'not for us PUT 1,0 'no common ram info on routine RUN Prog 'run correct prog gotname: usname =argl IF usname=US_null THEN lookstack usname =arg1 dousname: 'debug?usname IF usname>US_last THEN RUNX 'back to calling pgm GOSUB usname2usrules 'find where it is in DATA GET 3,usyawn0 'update variable GET 4,usyawn1 'update variable GET 5,irentry GET 9,flags 'PROGRam start '------------- '---- Behaviour Controller - Ultrasonic Sensor ---- usbehave: usDO: usRptr =usrules READ usRptr,BOScmnd 'REAd default usRptr =usRptr +1 ustest: GOSUB usercmnd 'check ir 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,doprog 'read pointer to pgm/yvar usRptr =usRptr +1 READ usRptr,Activity 'read pointer to activity/ythresh 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 uscmnd 'inspect cmnd usg: IF usactiong="X" THEN ustest 'ignore it BOScmnd =usactiong 'action triggered uscmnd: LOOKDOWN BOScmnd,["YyIJ"],tempb BRANCH tempb,[setY,unsety,Incprog,Jumpprog] usmove: GOSUB tellBOSc 'otherwise do action GOTO usbehave 'do it setY: usyawn.LOWNIB(yindx) =usyawn.LOWNIB(yindx) +1 MAX 15 IF usyawn.LOWNIB(yindx)<=ythresh THEN ustest 'next rule usRptr =usRptr +6 'skip next rule if bored GOTO ustest unsety: usyawn.LOWNIB(yindx) =0 GOTO ustest '------------- RUN leaves ---------------------- RUN0: PUT 0,0 'so prog0 doesn't redirect RUN 0 '-------------------- RUNX: GET 0,retprog PUT 0,2 'debug ? retprog RUN retprog 'go to pgm retprog '-------------------- Incprog: arg =0 'initialise arg argl =usname GET 63,Prog 'put this Prog in arg.lownib(3) GOSUB push_arg 'so Activity knows where to return PUT 0,Prog 'so we return to this pgm PUT 1,Activity 'so program knows what to do 'debug ?Activity PUT 3,usyawn0 'save variable PUT 4,usyawn1 'save variable 'debug ?doprog RUN doprog '-------------------- Jumpprog: PUT 1,Activity 'so program knows what to do 'debug ?Activity 'debug ?doprog RUN doprog '------------- subroutines --------------------- push: 'stacks the single byte in letterbox=commonRAM62 GET 61,stkp '61 is stack pointer stkp =stkp -1 'next location GET 62,tempb 'read letterbox PUT stkp,tempb 'push byte PUT 61,stkp 'save pointer RETURN '------------------------------- pop: 'unstacks a single byte into letterbox=commonRAM62 GET 61,stkp '61 is stack pointer GET stkp,tempb 'pop byte stkp =stkp+1 MAX 61 'previous max top+1 PUT 62,tempb 'byte to letter box PUT 61,stkp 'save pointer RETURN '------------------------------- push_arg: 'stacks the word arg PUT 62,argl GOSUB push PUT 62,argh GOSUB push RETURN '------------------------------- pop_arg: 'unstacks the word arg GOSUB pop GET 62,argh GOSUB pop GET 62,argl 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 '-------------------- usercmnd:'listen to InfraRed co-processor and read Pendant btns GOSUB btns 'check pendant buttons IF btn<>0 THEN RUN0 'other command wanted irin =0 'set to idle value IF Firwatch=0 THEN irret SERIN irrx,i96n,100,irret,[irin] 'valid 0 - 8 irin =irin & $F 'mask high nibble timing data IF irin<>irentry AND irin<>0 THEN RUN0 'new ir command irret: RETURN '-------------------- btns: lcdbtns =0 'set a default value SEROUT lcdtx,i96n,[27,"K0"] 'ask LCD for button status SERIN lcdrx,i96n,200,nolcd,[lcdbtns] 'get byte nolcd: btn =NCD lcdbtns.LOWNIB '0 - 4 RETURN '-------------------- usname2usrules: 'get the Rptr for usrules Dptr =usname *2 'expand to read words GOSUB read_arg 'read ptr for rule into Arg usrules =Arg 'set Rptr RETURN '-------------------- read_arg: READ Dptr,arg1 Dptr =Dptr +1 READ Dptr,arg2 Dptr =Dptr +1 RETURN '-------------------- tellBOSc: 'debug cr,BOScmnd,cr GOSUB usercmnd 'check ir PAUSE 10 'give the BOS time to go into receive mode IF BOSbusy=1 THEN tellBOSc 'wait until BOS not busy SEROUT BOStx,i96n,[BOScmnd] RETURN '------------------------ end program -------------------------