'>Hxtr4-3.BSx ' {$PBASIC 2.5} '============================================================== 'Milford Instruments http://www.milinst.com 'Hextor Design, Bug Commander - Behavioural Operating System 'and the Behavioural Controller programs by David Buckley '================================================================ '{$stamp BS2Sx} DEBUG "prog3" '============================================================== 'Hxtr4-0.BSx - Main 'Hxtr4-1.BSx - Activity sequencer 'Hxtr4-2.BSx - US Behaviour 'Hxtr4-3.BSx - Direct Control 'Hxtr4-4.BSx - Prog4 'Hxtr4-5.BSx - Prog5 'Hxtr4-6.BSx - LCD menu 'Hxtr4-7.BSx - Utils- LCD control and IR remote 'Hxtr4-Doc.BSx - Meta Commands and Program Headers '============================================================== 'New '--- ' 'Required knowledge: ' 'Meta command: 'BOS Commands: 'Common RAM: 'Constants: 'Variables: deleted 'FwarmIR var flags.bit4 'write6,write7 'Routine: 'Subroutine: 'Labels: 'Sequence: 'usRules: 'Behaviour: '============================================================== 'History '------- '05Nov13 PicAxeIR instead of MilinstIR. usercmnd: rewritten for IR action 'Hxtr4 '============================================================== 'Experiment with '============================================================== '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 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 '-------------------------------------------------------------- 'common RAM '0 '1 Program option - Activity to do, USrules to do, Options '2 '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 mopts - active menu OAUIP '8 menc - active menu displayed choice '9 mendo - selected choice '10 flags - various flag bits ' bit0=irwatch - 0=>ignore IR signal, 1=>act on it ' bit1=useLEDs - 0=>ignore LED cmnds, 1=>enable LEDs ' (for 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 '11 Dprog - Default program to run '12 Dmenc - Default option '13 lcmnds - number of learned BOScmnds Prog7LCDmenu '14 BOSver - BOS version '15 BOScmnd for Learning and Playing - to pass between Prog5 and Prog7 '16 Act - current Act being Learned/Played - used by Prog5/7 '17 pAct - current step in Act - used by Prog5 '18 IRpage '20 stkend - stack end 'prog1: stack ,, 'prog2: stack ,, 'prog7: stack ,, '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 Rled CON 8 'pin high turns on right green led Lled CON 9 'pin high turns on left red led ustx CON 14 'pin ultrasonic commands, i96n usrx CON 15 'pin ultrasonic data in, i96n i96n CON 16624 '9600 baud, 8 bit, no parity, inverted tcccpace CON 10 'transmit pace time '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 arg VAR Word 'prog1: stack ,, 'prog2: stack ,, 'prog7: stack ,, 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 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 stkp VAR Byte 'point to top of CommonRam stack retprog VAR Byte 'program to return to irentry VAR Byte 'irin on entry to pgm 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 'read1237 1=>watch for IR 'Fuseleds var flags.bit1 'future prog1 'Fusefeel var flags.bit2 'future prog1 'FusekUS var flags.bit3 'future prog2 'Flearn var flags.bit4 'write7 not saved to CRAM 'Fflagsvalid var flags.bit7 'read1/write1 init: HIGH epower 'keep electronics on GET 5,irentry GET 10,flags '---- 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 usercmnd 'check ir and buttons, only exit from this prog BOSdo: BOScmnd =0 arg1 =$90 'speed =9, servo =0 arg2 =200 'position =200 GOSUB tellBOSccc 'move 0 FOR arg2 =0 TO 9 'use arg2 as a temp variable BOScmnd =1 arg1 =0 'servo =0 GOSUB tellBOSccc 'ask position GOSUB BOSreply1 DEBUG"pulsing", BIN8 BOScmnd,BIN8 arg1,CR PAUSE 20 NEXT BOScmnd =4 GOSUB tellBOSccc 'ask pulsing GOSUB BOSreply2 DEBUG"pulsing", BIN8 BOScmnd,BIN8 arg1,CR BOScmnd =5 arg1 =0 GOSUB tellBOSccc 'stop pulsing PAUSE 1000 BOScmnd =4 GOSUB tellBOSccc 'ask pulsing GOSUB BOSreply2 DEBUG"pulsing", BIN8 BOScmnd,BIN8 arg1,CR BOScmnd =0 arg1 =$30 arg2 =50 GOSUB tellBOSccc 'move 0 PAUSE 150 BOScmnd =3 GOSUB tellBOSccc 'ask finished GOSUB BOSreply2 DEBUG"finished", BIN8 BOScmnd,BIN8 arg1,CR PAUSE 2000 BOScmnd =3 GOSUB tellBOSccc 'ask finished GOSUB BOSreply2 DEBUG"finished", BIN8 BOScmnd,BIN8 arg1,CR GOTO BOScon '------------- RUN leaves ---------------------- RUN0: RUN 0 '-------------------- RUNX: 'usercmnd: 'GET 0,retprog 'PUT 0,3 PUT 1,0 'no activity 'RUN retprog 'go to pgm retprog RUN 0 '-------------------- runstack: GET 61,stkp IF stkp=61 THEN RUNX 'nothing else to do GOSUB pop_arg 'no name so check stack IF Prog=3 THEN BOScon GOSUB push_arg 'not for us PUT 1,0 'no common ram info on routine RUN Prog 'run correct prog '------------- subroutines --------------------- 'usget:'talk to UltraSonic co-processor usRange =0 'invalid value, used if no repyl usout: SEROUT ustx,i96n,["T",usdir] PAUSE 500 'give servo time to get there SEROUT ustx,i96n,["T",usdir] 'ask again SERIN usrx,i96n,1500,us1,[usRange] 'jump if no reply DEBUG DEC usdir," ",DEC usRange," ",CR us1: 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 =irentry 'set to entry value IF Firwatch=0 THEN irret SERIN irrx,i96n,50,irret,[irin] IF irin=irentry THEN irret PUT 5,irin 'new ir command RUN 7 'do IR cmnd 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 '-------------------- 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: 'stacks the single byte in letterbox=commonRAM62 GET 61,stkp '61 is stack pointer stkp =stkp -1 MIN 20 'next location GET 62,tempb 'read letterbox PUT stkp,tempb 'push byte 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 '-------------------- tellBOSccc: GOSUB usercmnd 'check ir PAUSE 10 'give the BOS time to go into receive mode IF BOSbusy=1 THEN tellBOSccc SEROUT BOStx,i96n,tcccpace,[BOScmnd,arg1,arg2] 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 -------------------------