'>Hextor21.BSx
'==============================================================
'{$stamp BS2Sx}
'==============================================================
'Introduction of a facility to link sequences by means of the
'"a" Meta command similar to GOTO.
'
'Because we are running out of room
'the section in BOScon between
'**** make space delete start
'and
'**** make space delete end
'has been deleted
'
'New
'---
'In a sequence the meta command a,word 'sequence-name',
'permits 'sequence-name' to be jumped to.
'
'Introduction of word 'arg' with arg1 and arg2 redefined as its
'low and high bytes. Arg just used as temp storage at Sgo
' 
'new subroutine readword reads two bytes of DATA
'
'Required knowledge: 
'
'Meta command: 'a' now used in sequences
'Common RAM: 
'Constants:
'Variables:  arg, arg1 and arg2 redefined
'Routine:	 
'Subroutine: dosequence modified at Scon and Sa, read_arg
'Sequence:	 wiggle2 is linked to the end of wiggle3 by 'a'
'usRules:	
'Behaviour: 
'==============================================================
'Experiment with Meta command 'a'.
'==============================================================
'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
'--------------------------------------------------------------
'Sequences in DATA statments
'---------------------------
'Meta commands -
'"a",word <name> jump to sequence 'name'
'
wiggle1		DATA "UDFBCRLOBPMz"
wiggle2		DATA "UFFBBz"
wiggle3		DATA "UFRRLLBa",word wiggle2,"z"
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
'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 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


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
'		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,"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
'==============================================================
'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,"Beg             ",0
dm3	data	1,"Wiggle 3        ",0
dm4	data	1,"Setting 1       ",0
dm5	data	1,"Setting 2       ",0
dm6	data	1,"US corner       ",0
dm7	data	1,"US Hello        ",0
dm8	data	1,"BOS Control     ",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	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
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	8	'number of menu options
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

'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:
'---- Main Behaviour Controller - Infra-Red/Pendant buttons ---
ircon:toggle Rled
	pause 50
	mendo =0
	gosub lcdmenu		'returns butndo

case:	if mendo=1 then wig1		'key/mendo 1
	if mendo=2 then dobeg		'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 douscorner	'key/mendo 6
	if mendo=7 then doushello	'key/mendo 7
	if mendo=8 then BOScon		'key/mendo 8

	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

dobeg:Sptr =beg
	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

'---- Behaviour Controller - Ultrasonic Sensor ----
douscorner:usrules =uscorner
	lcdmsg =dm6		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub irget			'check ir
	if irin=6 then douscorner	'wait for idle signal
	goto usbehave
dousbeg:usrules =usbeg
'	lcdmsg =dm7		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub irget			'check ir
	if irin=7 then dousbeg		'wait for idle signal
	goto usbehave
doushello:usrules =ushello
	lcdmsg =dm6		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	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
	gosub btns		'check pendant buttons
	if btn<>0 then start	'other command wanted
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:
	lcdmsg =dm8	:gosub lcd
	gosub irget			'check ir
	gosub btns			'check pendant buttons
	gosub irget			'check ir
	if irin=8 then BOSdo	'for us
	if irin<>0 then start	'other ir command received
	if btn<>0 then start	'other command wanted
BOSdo:BOScmnd =0
'**** make space delete start
'**** make space delete end
	goto BOScon

'------------- 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,dm8],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="a" then Sa
	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:	gosub read_arg
	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
Sa:	gosub read_arg		'read new Sptr
	Sptr =arg			'make Stpr=new Sptr
	goto dosequence		'do new sequence
doneS:return
'--------------------
read_arg:
	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
'--------------------
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 -------------------------