'>Hextor14.BSx '============================================================== '{$stamp BS2Sx} '============================================================== 'Introducing direct control of the servos, 'dosequence extended 'so the three bytes for direct control can be included in the 'sequences in DATA statements. 'Also introducing the command "W". ' 'New '--- 'The subroutines necessary for direct control of servos. 'Key 7 used for the sequence 'dobeg'. ' 'A test in the lcd ir routine to see if the IR code has changed 'so we don't do the same sequence twice for the same keypress. ' 'Sptr the pointer to the routines is a byte so it can't point 'to locations beyond 255. 'lcdmsg the pointer to the LCD messages is a word so they can 'grow beyond 255. 'So the LCD messages should be the last DATA. ' 'Required knowledge: commands for direct control of the servos ' 'BOS Command: 3 byte direct control,W 'Constants: lcdoptions altered 'Variables: arg2, oldirin 'Routine: Scon as part of dosequence, directcon1 'Subroutine:BOSmsg3, BOSrepl1, BOSrepl2, dosequence extended ' lcdmenu altered for dm7, ' irget default value was 15 now 0=idle value 'Sequence: beg 'LCD text: dm7 '============================================================== 'Warning '------- 'If you have been keeping an eye on the memory map you 'will have seen that the DATA has been growing steadily towards '$FF. All the pointers to the DATA are byte variables so they 'can't address locations beyond $FF. Modifying the DATA for 'this program so it extends past $FF will lead to intersting 'but non meaningful results. In TB_15.BS2 all the DATA 'pointers are changed to word variables. 'You will also note that the sequence 'wiggle2' is not used in 'this program, it could be deleted but may be useful in the 'future. Deleting sequences can lead to unpredictable results 'since you have to be absolutely certain that they are not used 'anywhere in the program, if in doubt change them to the single 'byte "z" for a no action sequence. 'Writing control sequences illustrates the importance of a good 'education, if the memory is first filled up with junk 'sequences then later it becomes very difficult to assign 'meaningful names to useful sequences. Changing the contents of 'a sequence can lead to all sorts of strange behaviour. '============================================================== '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 '============================================================== 'Experiment with different commands to the servos. '============================================================== 'Sequences in DATA statments '--------------------------- 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 "RRRRRRRRRRRRz" 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 'Ultrasonic sensor '----------------- '"a" - Action sequence 'Rules - DATA direction,range,action if greater,action if less usaction DATA "S" 'default action ussequence DATA tround 'Action-sequence usrules DATA usF, 20,"XB" 'back if <=20 DATA usR2, 20,"XL" 'left if <=20 DATA usL2, 20,"XR" 'right if <=20 DATA usF, 25,"Xa" 'Action if <=25 DATA usR2, 25,"Xa" 'Action if <=25 DATA usL2, 25,"Xa" 'Action if <=25 DATA usF, 80,"XF" 'forward if <80 DATA usR2, 80,"XR" 'right if <80 DATA usL2, 80,"XL" 'left if <80 DATA 0 'end of list '============================================================== 'LCD commands. 'For more detailed information see the ILM-216 User Manual. 'Function ASCII '-------- ----- 'Null 0 'Cursor home 1 'Hide cursor 4 'Show underline cursor 5 'Show blinking-block cursor 6 'Bell (not implemented with Hextor LCD) 7 'Backspace 8 'Horezontal tab (4 columns) 9 'Smart linefeed (cursor down one line) 10 'Vertical tab (cursor up one line) 11 'Formfeed (clear screen) 12 'Carriage return 13 'Backlight on 14 'Backlight off 15 'Accept cursor position entry 16 ' 64 is the start of the first line ' 80 is the start of the second line 'Format right aligned text 18 'Escape (ESC; start multipart instruction) 27 '==========LCD messages - finish each message with Null ======= dbtxt data 12,4,16,80,"back < > ok ",0 dbusy data 16,76,"busy",0 dm0 data 1,"OPTIONS ",0 dm1 data 1,"Wiggle 1 ",0 dm2 data 1,"Wiggle 2 ",0 dm3 data 1,"Wiggle 3 ",0 dm4 data 1,"Setting 1 ",0 dm5 data 1,"Setting 2 ",0 dm6 data 1,"US wander ",0 dm7 data 1,"Beg ",0 '-------------------------------------------------------------- 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 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 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 byte 'pointer to DATA sequence string arg1 var byte 'command argument arg2 var byte 'command argument irin var byte 'infra-red command oldirin var byte 'old infra-red command 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.lownib '0 - 4 current button state oldbtn var nib '0 - 4 last button state lcdmsg var word 'pointer to current char in text lcdchar var byte 'character to send to LCD btnc var nib 'current BuTtoN Choice from menu mendo var nib 'selection chosen by OK lcdoptions con 7 'number of menu options usdir var byte 'us servo direction, 255=left 0=right usRange var byte 'us servo range, 15-255 usRptr var byte '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 '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 pause 100 'no point in listening too often if irin<>0 then init1 'wait for ir to initialise debug cls pause 1000 'give time for lcd to get ready 'IR circuit sometimes starts up with a false key of 2 gosub irget pause 100 'no point in listening too often if irin<>0 then init1 'wait for false key to end lcdmsg =dbtxt :gosub lcd 'write button text 'Program start '------------- start:toggle Rled debug boscmnd pause 50 mendo =0 gosub lcdmenu 'returns mendo case: if mendo=1 then wig1 'key/mendo 1 if mendo=2 then wig2 'key/mendo 2 if mendo=3 then wig3 'key/mendo 3 if mendo=4 then set1 'key/mendo 4 if mendo=5 then set2 'key/mendo 5 if mendo=6 then uswander 'key/mendo 6 if mendo=7 then dobeg 'key/mendo 7 toggle Lled pause 50 goto start '------------- leaves -------------------------- wig1: Sptr =wiggle1 lcdmsg =dm1 :gosub lcd lcdmsg =dbusy :gosub lcd gosub dosequence goto start wig2: Sptr =wiggle2 lcdmsg =dm2 :gosub lcd lcdmsg =dbusy :gosub lcd gosub dosequence goto start wig3: Sptr =wiggle3 lcdmsg =dm3 :gosub lcd lcdmsg =dbusy :gosub lcd gosub dosequence goto start set1: Sptr =setting1 lcdmsg =dm4 :gosub lcd lcdmsg =dbusy :gosub lcd gosub dosequence goto start set2: Sptr =setting2 lcdmsg =dm5 :gosub lcd lcdmsg =dbusy :gosub lcd gosub dosequence goto start dobeg:Sptr =beg lcdmsg =dm7 :gosub lcd lcdmsg =dbusy :gosub lcd gosub dosequence goto start uswander: lcdmsg =dm6 :gosub lcd lcdmsg =dbusy :gosub lcd gosub btns 'check pendant buttons if btn<>0 then start 'other command wanted gosub irget 'check ir if irin=6 then usdo 'for us if irin<>0 then start 'other ir command received usdo: read usaction,BOScmnd 'read default read ussequence,Sptr 'read pointer to sequence usRptr =usrules 'point to desired rules 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 '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="a" then sact gosub tellBOS 'now do action goto uswander 'do it sact: gosub dosequence goto uswander '------------- subroutines --------------------- 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 '-------------------- lcdmenu: lcdit:lookup btnc,[dm0,dm1,dm2,dm3,dm4,dm5,dm6,dm7],lcdmsg gosub lcd lcdb: gosub irget if irin=oldirin then mbtns oldirin =irin if irin=0 then mbtns mendo =irin return 'with ir cmdn mbtns:gosub btns if btn=oldbtn then lcdb oldbtn =btn if btn=2 then lcd2 if btn=3 then lcd3 if btn=4 then lcd4 goto lcdb lcd2: btnc =1 +btnc -1 min 1 -1 :goto lcdit '+1 -1 => don't go through 0 lcd3: btnc =btnc +1 max lcdoptions :goto lcdit lcd4: mendo =btnc lcdmsg =dbusy :gosub lcd gosub nobtns return '-------------------- lcd: read lcdmsg,lcdchar if lcdchar=0 then lcdret 'if 0 then end of msg serout lcdtx,i96n,[lcdchar] lcdmsg =lcdmsg +1 'point to next character goto lcd lcdret: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 '----------------------- nobtns:lcdbtns =0 'wait until no buttons are pressed nbtn: serout lcdtx,i96n,[27,"K0"] 'ask LCD for button status serin lcdrx,i96n,200,nolcd,[lcdbtns] 'get byte if lcdbtns.lownib<>0 then nbtn return '-------------------- irget:'listen to InfraRed co-processor irin =0 'set to idle 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<6 then Scon arg1 =255 lookdown BOScmnd,["srml"],arg1 if arg1=255 then t1 'BOScmnd not srm or l, jump to t1 read Sptr,arg1 'read it Sptr =Sptr +1 gosub tellBOS1d 'send them goto dosequence t1: gosub tellBOS goto dosequence Scon: read Sptr,arg1 Sptr =Sptr +1 read Sptr,arg2 Sptr =Sptr +1 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 doneS: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 '-------------------- 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 -------------------------